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ABSTRACT 


The  Flexible  Spacecraft  Simulator  (FSS)  at  the  Naval  Postgraduate  School 
was  modified  by  replacing  the  flexible  appendage  with  a  two  link  robotic 
manipulator.  This  experimental  setup  was  designed  to  simulate  motion  of  a 
spacecraft  about  the  pitch  axis.  The  spacecraft  consists  of  a  main  body,  a  two 
link  manipulator,  and  momentum  wheel  actuator  to  control  the  pitch  attitude  of 
the  spacecraft.  Position  information  from  the  main  body  and  manipulators  was 
obtained  from  Rotary  Variable  Displacement  Transducers  (RVDT).  The  equations 
of  motion  were  developed  assuming  that  the  main  body  and  manipulator  were 
rigid  bodies.  The  resulting  coupled,  nonlinear,  time  invariant  equations  of  motion 
were  used  to  analyze  the  dynamics  and  kinematics  of  the  main  body  and 
manipulator  as  well  as  the  interaction  between  the  main  body  and  manipulator. 

Three  different  control  strategies  were  developed  using  Lyapunov's  Second 
or  Direct  Method.  With  the  first  controller,  simple  linear  feedback  of  position  and 
velocity  information  with  constant  gains  was  used  to  position  the  manipulator 
and  stabilize  the  main  body.  A  fifth  order  polynomial  was  used  to  generate  a 
reference  trajectory  for  the  second  controller.  This  trajectory  was  used  in 
conjunction  with  a  tracking  controller  to  position  and  stabilize  the  system.  In  the 
third  controller,  a  near-minimum- time  technique  was  used  to  generate  a  reference 
trajectory.  This  reference  trajectory  was  employed  using  a  tracking  controller 
similar  to  that  used  in  the  polynomial  reference  controller. 
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I.  INTRODUCTION 


During  the  past  few  years  there  has  been  a  significant  increase  in  the  use  of 
robotics.  Applications  range  from  performing  routine  tasks  in  manufacturing  to 
deep  sea  and  interplanetary  space  exploration.  The  interplanetary  and 
extraterrestrial  environment  has  become  the  focus  of  research  for  future  industrial 
development  and  scientific  exploration.  With  the  hazards  of  this  environment 
and  cost  of  manned  space  flight,  researchers  will  become  increasingly  dependent 
upon  robotics  for  assembly,  service,  and  repair  of  equipment  in  space  as  well  as 
the  exploration  of  space  itself.  Due  to  the  requirements  for  terrestrial  and  space 
applications,  there  has  been  a  significant  increase  in  theoretical  and  experimental 
research  in  the  areas  of  robotic  dynamics  and  control. 

Space  based  robotic  applications  differ  from  terrestrial  applications  in  one 
important  area.  For  the  space  based  applications,  no  support  is  provided  to 
stabilize  the  manipulator.  A  space  based  manipulator,  when  repositioned,  imparts 
moments  and  forces  on  the  spacecraft.  In  addition,  there  is  no  friction  to  dissipate 
energy  that  is  added  to  the  system.  To  counteract  these  forces  and  moments,  an 
attitude  control  system  normally  consisting  of  thrusters  in  combination  with  a 
momentum  wheel  is  used  to  stabilize  the  spacecraft.  Control  system  problems  are 
exasperated  as  the  mass  of  the  load  that  is  being  positioned  becomes  larger  in 
relation  to  the  mass  of  the  spacecraft.  It  is  this  interaction  between  spacecraft  and 
the  motion  of  a  manipulator  that  warrants  further  research. 

The  primary  objective  for  any  control  system  is  to  remain  stable  over  a  wide 
range  of  operating  conditions  while  still  providing  adequate  levels  of 
performance.  It  is  desired  to  meet  this  objective  in  the  face  of  hardware 
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characteristics,  changing  loads  as  well  as  unmodeled  disturbances  and  system 
dynamics.  These  requirements  and  restrictions  present  the  control  system  design 
engineer  with  significant  challenges. 

Before  the  system  can  be  analyzed,  the  equations  of  motions  must  be 
determined.  The  equations  of  motion  for  a  robotic  system  can  easily  be  developed 
through  Lagrange's  equations  and  are  in  the  form  of  a  set  of  second  order 
differential  equations.  These  equations  are  coupled  and  nonlinear  with 
trigonometric  and  higher  order  terms.  Attempts  to  simplify  these  equations  result 
in  equations  of  motion  that  are  valid  over  a  limited  range  of  motion  or  for  specific 
boundary  conditions.  The  current  trend  in  trajectory  control  requires  highly 
nonlinear  maneuvers  that  are  valid  over  a  wide  range  of  applications  and 
operating  conditions.  These  requirements  dictate  that  the  full,  nonlinear 
equations  be  used  to  describe  the  motion  of  the  system.  With  the  introduction  of 
the  nonlinear  equations  of  motion,  many  traditional  tools  in  control  theory  used 
to  analyze  linear,  time- invariant  systems  are  not  available  or  are  meaningless. 

Recently,  research  by  Junkins  [Ref.  1]  and  Bang  [Ref.  2]  has  revived  interest 
in  using  Lyapunov's  second  method  for  a  flexible  structure  control  system  design. 
This  technique  is  very  attractive  because  it  can  be  applied  to  nonlinear,  time 
invariant,  systems  with  guaranteed  stability  for  a  wide  range  of  conditions.  An 
important  feature  of  Lyapunov's  second  method  is  the  freedom  to  select  the 
Lyapunov  function  and  the  corresponding  feedback  control  law.  The  Lyapunov 
function  can  be  selected  based  on  physical  insight  and  the  control  law  can  be 
selected  to  ensure  that  the  system  is  stable.  The  Lyapunov  function  must  be 
positive  definite  and  is  normally  related  to  the  system  energy  for  a  large  class  of 
mechanical  natural  systems.  The  control  law  can  be  selected  such  that  the 
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Lyapunov  function  or  system  energy  will  always  decrease  to  zero  or  some 
equilibrium  point. 

The  purpose  of  this  thesis  is  to  apply  a  general  methodology  for  finding 
Lyapunov  stable  control  laws  for  stabilizing  the  spacecraft  main  body  while 
controlling  a  two  link  manipulator  attached  to  that  spacecraft.  A  complete 
description  of  the  experimental  setup  is  discussed  in  Chapter  II.  Topics  include 
the  physical  characteristics  of  the  manipulator,  main  body,  actuators,  sensors,  test, 
simulation,  and  data  collection  equipment.  In  Chapter  III,  the  coordinate  systems 
and  the  equations  of  motion  are  developed.  Three  different  control  strategies 
were  developed  using  Lyapunov’s  Second  or  Direct  Method.  With  the  first 
controller,  simple  linear  feedback  of  position  and  velocity  information  with 
constant  gains  was  used  to  position  the  manipulator  while  stabilizing  the  main 
body.  A  fifth  order  polynomial  was  used  to  generate  a  reference  trajectory  for  the 
second  controller.  This  trajectory  was  used  in  conjunction  with  a  tracking 
controller  to  position  and  stabilize  the  system.  In  the  third  controller,  a  near¬ 
minimum-time  technique  was  used  to  generate  a  reference  trajectory.  This 
reference  trajectory  was  employed  with  a  tracking  controller  similar  to  that  used 
in  the  polynomial  reference  controller.  Simulation  results  are  presented  in 
Chapter  IV.  Chapter  V  includes  a  summary  of  the  conclusions  as  well  as  topics 
for  future  research. 
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II.  EXPERIMENTAL  SETUP 


A.  SPACE  ROBOTICS  SIMULATOR  DESCRIPTION 

The  Spacecraft  Robotics  Simulator  (SRS)  is  a  modification  to  the  Flexible 
Spacecraft  Simulator  (FSS)  used  for  previous  work  by  Hailey  [Ref.  3],  and 
Watkins  [Ref.  4].  The  FSS  was  modified  by  removing  the  flexible  appendage 
and  replacing  this  appendage  with  a  two  link  manipulator.  The  SRS  consists  of  a 
central  main  body  with  a  two  link  robotic  manipulator.  Pitch  axis  control  of  the 
main  body  is  provided  by  a  single  momentum  wheel  driven  by  an  electric  servo¬ 
motor.  The  central  body  was  constrained  to  rotational  motion  only  by  an  I-beam 
mounted  over  the  over  the  granite  table.  The  main  body  and  manipulator  were 
supported  by  air  bearings  that  float  upon  a  thin  cushion  of  air  on  an  optical 
quality  granite  surface.  Each  of  the  two  links  were  positioned  via  geared  DC 
servo-motors.  A  Rotary  Variable  Displacement  Transducer  (RVDT)  was  used  to 
obtain  position  information  at  each  joint  for  position  feedback.  This  setup  was 
designed  to  simulate  a  zero-gravity  environment  in  two  dimensions.  The  SRS  is 
depicted  in  Figure  2.1 . 

B.  EXPERIMENT  DESCRIPTION 

The  SRS  is  composed  of  the  following  major  components: 

•  Spacecraft  Main  Body 

•  Two  Link  Robotic  Manipulator 

•  RVDT  Position  Sensors 

•  Granite  Table 

•  Electrical  Power  Supplies 

•  AC- 100 

•  VAX  3100  Series  Computer 
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Figure  2.1  Spacecraft  Robotics  Simulator 


1.  Spacecraft  Main  Body 

The  spacecraft  main  body  consists  of  a  rigid,  7/8  inch  thick,  30  inch 
diameter,  aluminum  disk.  The  main  body  is  designed  to  simulate  the  two 
dimensional  planar  motion  of  a  spacecraft  about  its  pitch  axis.  The  main  body  is 
supported  by  three  air  bearings  spaced  at  120  degree  intervals.  Each  of  the 
bearings  is  capable  of  supporting  a  load  of  60  pounds.  A  fourth  air  bearing 
supported  by  an  overhead  I-beam  constrains  the  spacecraft  main  body  to 
rotational  motion  only.  The  air  bearings  are  designed  to  float  the  spacecraft  main 
body  on  a  thin  film  of  air  supplied  by  an  external  air  source.  A  RVDT,  model 
R30D,  was  connected  to  the  rotor  of  the  air  bearing  by  a  bellow-type  device. 


The  RVDT  was  manufactured  by  Schaevitz  Sensing  Systems  was  used  to  measure 
angular  displacements  of  the  spacecraft  main  body. 

Attached  to  the  spacecraft  main  body  was  a  10.7  kilogram  steel 
momentum  wheel  and  servo  motor.  The  momentum  wheel  was  designed  to  apply 
a  torque  to  the  main  body  by  increasing  or  decreasing  the  angular  velocity  of  the 
momentum  wheel.  The  motion  of  the  spacecraft  about  its  pitch  axis  was 
controlled  by  the  torque  generated  by  this  momentum  wheel.  The  servo  motor, 
model  JR16M4CH/F9T  used  to  drive  the  momentum  wheel  was  manufactured  by 
PMI  industries.  Characteristics  of  this  motor  are  presented  in  Table  2.1 . 


TABLE  2.1  Momentum  Wheel  Actuator  Characteristics 


Characteristic 

Units 

Manufacturer 

PMI  Motion  Technologies 

Model  Number 

JR16M4CH-1 

Rated  Speed 

3000 

Rated  Power 

horse  power 

1.4 

Rated  Torque 

inch-pound 

31.85 

Rated  Current 

amps 

7.79 

Rated  Voltage 

volts 

168 

Outside  Diameter 

inches 

7.4 

Height 

inches 

4.5 

Weight 

pounds 

17.5 

An  integral  analog  tachometer,  model  ARS-C121-1A,  manufactured  by  Watson 
Industries,  Inc.  was  mounted  on  the  servo-motor  to  measure  angular  velocity. 
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A  more  detailed  description  of  the  motor,  momentum  wheel,  and  spacecraft  main 
body  can  be  found  in  [Ref.  3],  and  [Ref.  4], 

2.  Two  Link  Manipulator 

Attached  to  the  main  body  was  a  two  link  manipulator.  Components  for 
the  manipulator  were  designed  and  built  by  the  Aeronautics  and  Astronautics 
Department  at  the  Naval  Postgraduate  School.  All  components  for  the  arm  were 
manufactured  from  7075  and  6061  series  aluminum.  All  components  were 
connected  with  SAE  grade  8  medium  carbon  chrome  alloy  cap  screws.  A  picture 
of  the  manipulator  can  be  found  in  Figure  2.2. 


Figure  2.2  Two  Link  Manipulator 
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The  links  of  the  manipulator  were  positioned  by  two  geared,  servo-disk  motors 
manufactured  by  PMI.  A  third  motor,  identical  to  the  first  two  actuators  and  was 
mounted  on  the  tip  of  link  two  for  the  purpose  of  orienting  a  simulated  tool  or 
pointing  some  type  of  antenna.  Characteristics  for  the  link  actuators  are 
presented  in  Table  2.2.  At  each  joint,  a  RVDT,  model  R30D,  manufactured  by 
Schaevitz  was  used  to  measure  relative  angular  displacement. 


TABLE  2.2  Link  Actuator  Characteristics 


Characteristic 

Units 

Manufacturer 

PMI  Motion  Technologies 

Model  Number 

9FGHD 

Rated  Speed 

22 

Rated  Torque 

inch-pound 

80 

Rated  Current 

amps 

5.6 

Rated  Voltage 

volts 

12 

Outside  Diameter 

inches 

4.75 

Height 

inches 

3 

Weight 

pounds 

3.2 

The  power  supplies  used  to  drive  the  actuators  of  the  manipulators  were 
manufactured  by  Kepco  Inc.  of  Flushing,  New  York.  The  Kepco  series  BOP 
Bipolar  Power  Supplies  were  designed  to  be  fast,  programmable,  fully  dissipative, 
linear  amplifiers.  The  BOP  power  supply  is  an  all  solid-state  design,  featuring 
integrated  circuit  operational  amplifiers  in  the  control  circuit  section  and  silicon 
power  transistors  mounted  on  special  fan-cooled  heat  sinks  in  the  complementary 


8 


power  stage.  Characteristics  for  the  Kepco  model  BOP  20-10  power  supply  are 
presented  in  Table  2.3. 


TABLE  2.3  Kepco  Power  Supply  Characteristics 


watts 

200 

amps 

5.5 

flPIPili 

watts 

540 

DC.  Output  Range 

volts 

±20 

MM 

±  10 

volts/volt 

2 

1 

1 

Bandwidth 

kilohertz 

1 8  (voltage  mode) 

kilohertz 

6  (current  mode) 

HH 

microseconds 

20  (voltage  mode) 

microseconds 

60  (current  mode) 

Recovery  Step  Load 

microseconds 

80  (voltage  mode) 

microseconds 

20  (current  mode) 

The  BOP  can  be  operated  in  either  the  voltage  or  current  mode  through  two 
bipolar  control  channels.  These  modes  are  manually  selectable  through  the  front 
control  panel  or  through  remote  signals.  Each  of  the  principal  control  channels  is 
protected  by  bipolar  limit  circuits.  All  control  and  limit  channels  are  connected  to 
the  output  stage  via  an  "Exclusive-Or"  gate  so  that  only  one  channel  is  in  control 
of  the  BOP  output  at  any  one  time.  The  BOP  output  can  be  programmed  over  its 
full  output  range  by  a  ±10  volt  signal  applied  to  either  one  of  the  inputs  to  the 
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voltage  or  current  channel.  The  limit  control  channels  can  be  remotely  controlled 
by  a  0  to  +10  volt  signal  applied  to  there  respective  inputs. 

3.  Granite  Table 

The  entire  mechanical  assemblage,  including  the  main  body  and 
manipulator  were  supported  by  air  bearings  that  float  on  a  thin  cushion  of  air  on  a 
granite  table  with  dimensions  of  8  feet  by  6  feet  by  10.5  inches  thick.  The  surface 
of  the  table  was  highly  polished  to  optical  quality  grade  A  (0.001  inch  peak  to 
valley).  The  smooth  surface  allows  the  air  bearings  to  float  freely  over  the  surface 
of  the  granite  table  to  minimize  the  effects  of  friction  on  the  motion  of  the  main 
body  and  manipulator.  The  granite  table  was  carefully  leveled  to  eliminate 
gravity  induced  accelerations.  The  mass  of  the  table  provided  an  extremely  stable 
platform  upon  which  to  conduct  the  experiments. 

4.  AC  -100 

The  AC-100  is  a  microprocessor  based,  programmable,  real  time  control 
system  manufactured  by  Integrated  Systems  International,  Inc.  of  San  Jose, 
California.  The  AC- 100  was  designed  to  automate  the  development  of  real-time 
systems  by  combining  graphical  modeling  tools  with  a  real-time  controller.  In 
addition  to  modeling  and  controlling,  the  AC- 100  was  also  capable  of  data 
collection  and  storage.  The  AC- 100  consists  of  the  following  major  components: 
Intel  80386  Application  Processor,  Intel  80386  Multibus  II  Input  /  Output 
Processor,  Intel  80386  Communication  Processor,  Intel  80387  Coprocessor, 
Weitek  3167  Coprocessor,  Analog  To  Digital  and  Digital  To  Analog  Digital  Data 
Translation  DT2402  Input/  Output  Board,  Two  -  INX-04  Encoder  and  Digital  To 
Analog  Servo  Boards,  Ethernet  Interface  Module,  and  Cabinet  Enclosure  and 
Power  Supply 
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The  software  tools  used  with  the  AC- 100  include  a  Design  Package  and 
a  Run-Time  Package.  The  Design  Package  included  Matrixx,  System  Build,  and 
Auto  Code.  These  tools  are  used  for  analysis,  design,  and  code  generation 
respectively.  The  Run-Time  Software  Package  provides  the  Graphical  User 
Interface  (GUI)  cross  compiler,  device  drivers,  data  acquisition,  and  Ethernet 
interface  required  to  run  software  code  generated  by  Auto  Code  on  the  AC- 100. 

5.  VAX  3100  Series  Computer 

The  VAX  3100  Series  Model  30  workstation  was  configured  with  8 
megabytes  of  main  memory,  a  19  inch  (diagonal)  color  monitor,  two  104  megabyte 
Winchester  hard  disks  and  a  mouse.  The  VAX  workstation  is  capable  of  2.8 
Million  Instructions  Per  Second  (MIPS). 

C  SYSTEM  INTEGRATION 

The  AC- 100  is  integrated  with  a  VAX  3100  Series  computer  via  the  Ethernet 
interface.  In  this  system,  the  VAX  3100  computer  was  used  to  analyze  and  design 
the  control  system.  Auto  Code  was  then  used  to  convert  the  final  control  system 
design  into  fully  compiled  and  linked  "C"  computer  code.  This  code  was  then 
down  linked  to  the  AC- 100  via  the  Ethernet  interface.  The  control  system,  in  the 
form  of  C  -  code  software  can  then  be  used  to  control  the  servo-amplifiers  which 
in  turn  were  used  to  drive  the  actuators  of  the  manipulator  in  real  time.  Manual 
control  of  the  system  is  still  provided  through  the  VAX  computer  acting  through 
the  Ethernet  interface.  A  block  diagram  of  the  integration  of  the  control  system 
for  the  two  link  manipulator  and  spacecraft  main  body  are  presented  in  Figure 
2.3.  Mass  and  inertia  characteristics  are  summarized  in  Table  2.4 
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Reaction 
^  Wheel 
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TABLE  2.4  Mass  And  Inertia  Characteristics 


mmm 

Characteristic 

Units 

Value 

Arm  1 

Mass 

kg 

2.09 

Inertia  ( 1 ) 

kg-m^ 

0.3102 

Inertia  (2) 

kg-m^ 

0.032 

Center  Of  Mass(3) 

cm 

36.45 

Arm  2 

Mass 

kg 

2.47 

Inertia  (1) 

kg-m^ 

0.3542 

Inertia  (2) 

kg-m^ 

0.054 

Center  Of  Mass(3) 

cm 

34.90 

Mass 

kg 

52.73 

Inertia  (1) 

kg-m^ 

0.3542 

Momentum  Wheel 

Mass 

kg 

10.67 

Inertia  (1) 

ke-m2 

0.0912 

Center  Of  Mass 

cm 

20 

Notes:  (1)  Moment  Of  Inertia  About  Arm  Axis  Of  Rotation. 


(2)  Moment  Of  Inertia  About  Center  Of  Mass. 


(3)  Center  Of  Mass  Location  With  Respect  To  Axis  Of  Rotation. 
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III.  ANALYTICAL  MODEL 


The  Spacecraft  Robotics  Simulator  (SRS)  consists  of  a  central  main  body  to 
which  is  attached  a  two  link  robotic  manipulator.  The  motion  of  the  main  body  is 
constrained  to  move  in  a  plane  resulting  in  a  system  with  three  degrees  of 
freedom.  This  planar  motion  constraint  greatly  simplifies  the  problems  associated 
with  the  derivation  of  the  equation  of  motion  and  control  system  design  yet  still 
retains  the  most  critical  analytical  elements.  The  manipulator  and  main  body  were 
modeled  as  rigid  structures.  Lagrange’s  equation  was  used  to  derive  the 
equations  of  motion.  This  section  will  first  describe  how  the  equations  of  motion 
were  derived  and  then  how  the  control  system  was  developed. 

A.  COORDINATE  SYSTEMS 

Before  the  spacecraft  attitude  and  manipulator  can  be  controlled  the 
dynamics  of  the  system  must  be  carefully  defined  and  understood.  The  first  step 
in  this  process,  is  to  determine  the  equations  of  motion  for  the  system. 

In  this  model,  the  main  body  is  constrained  to  rotational  motion  only.  Four 
different  coordinate  systems  were  used  for  this  analysis.  The  Nj,  N2,  N3  axis  for 
this  problem  was  fixed  in  inertial  space  coincident  with  the  axis  of  rotation  of  the 
mam  body.  The  coordinate  system,  xi,  yi,  zj,  is  fixed  in  the  main  body  with  the 
xi  axis  pointing  toward  the  attachment  point  for  link  1.  The  xi,  yi,  Zi  coordinate 
system  is  obtained  by  rotating  about  the  N3  axis  of  the  inertial  coordinate,  by  an 
angle  of  0i  .  In  a  similar  way,  the  coordinate  systems,  X2,  yi,  Z2»  is  fixed  at  the 
axis  of  rotation  of  link  1  and  points  toward  the  attachment  point  for  link  2.  The 
x2»  3  l-.  z2  coordinate  system  can  be  obtain  by  rotating  about  the  N3  axis  of  the 
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inertial  coordinate  system  by  an  angle  of  02-  The  coordinate  system,  X3,  y3,  Z3,is 
fixed  in  the  body  of  link  2  with  the  X3  axis  pointing  toward  the  end  point  of  link 
2.  This  coordinate  system  is  obtained  by  rotating  by  an  angle,  03  about  the  N3 
axis  of  the  inertial  system.  Coordinate  systems  are  presented  in  Figure  3  1 . 


Figure  3.1  Inertial  And  Local  Coordinate  Systems 
All  angles,  0j,  02,  and  63,  as  well  as  vectors  r  1 ,  r2,  and  r^  are  defined  in  terms 
of  inertial  coordinates  where  these  quantities  are  defined  in  Figure  3.1 
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Q*j  Center  Of  Mass 

o  Servo  Motor 


Figure  3.2  Inertial  Angles  And  Vectors 


— ♦  — * 

The  vectors  rj,  r2,  and  describe  the  location  of  the  center  of  mass  of  each  of 
the  manipulators  in  terms  of  inertial  coordinates.  The  angles  ©21,  ©31,  and  ©32 
represent  relative  displacements  of  the  joints  and  can  be  derived  from  the  inertial 
coordinates  0j,  ©2,  and  63  in  such  a  way  that  ©21  =  ©2  -  ©1,  ©31  =  ©3  -  ©1,  and 
©32  =  ©3  -  ©2- 
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B.  EQUATIONS  OF  MOTION 

A  momentum  wheel  was  used  to  apply  a  control  torque  to  the  main  body. 
The  motion  of  the  momentum  wheel  is  de-coupled  from  the  motion  of  the  main 
body.  It  is  assumed  that  the  momentum  wheel  is  only  an  actuator  that  is  used  to 
control  the  motion  of  the  main  body  and  counteract  torques  generated  by  the 
movement  of  the  manipulator. 

1.  Lagrange's  Equations  Of  Motion 

Lagrange’s  equation  for  n-dimensional  dynamic  systems  are  stated  as 

4^1  ~  =  Qi .  where  i  =  1, 2, 3,...,  n  (3.1) 

dt\dqi  /  dqi 

L  =  T-  V 

V  -  Potential  Energy 

T  -  Kinetic  Energy 

qi  -  Ith  Generalized  Coordinate 

qj  -  i111  Generalized  Velocity 

Qi  -  ith  Applied  Non  conservative  Force 
For  this  particular  problem,  the  system  requires  three  degrees  of  freedom 
to  describe  its  governing  equations  of  motion.  The  generalized  coordinates  and 
velocities  are  chosen  to  be  in  terms  of  inertial  coordinates  as  presented  below. 


q  =  101,02, 03)T={qbq2,  q3|T 

(3.2) 

q  =  {0i,  02,  03)T  =  (qi?  q2,  q3)T 

(3.3) 

2.  Kinetic  Energy 

The  first  step  is  to  determine  the  total  kinetic  energy  of  the  system.  The 
total  kinetic  energy  of  the  system  is  given  by 
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(3.4) 


T=t  Ti 

i=l 

Ti  =  ^  Ii  6i2  +  ^  mi  (fj  •  n)  (3.5) 

Ii  -  i111  Mass  Moment  Of  Inertia  About  Center  Of  Mass 
mi  -  i 115  Mass 

n  ?i  =  I  I  l?  tf©,  ej(y,  -yj  (3.6) 

i  =  1  i  =  1 

A  more  detailed  algebraic  procedure  can  be  found  in  Appendix  B. 

3.  Equation  Of  Motion 

Lagrange's  equations  can  be  applied  to  equation  (3.4)  and  the  terms  can 
be  arranged  in  matrix  form  so  that  the  final  form  can  be  written  as  (3.9). 


where 


and 


M(e)0  +  G(e,  o)  =  q 


M(0)  = 


mu  mi2  mi3 
m21  m22  m23 
rri3i  m32  m33 


mu  =  Ii  +  Li  [mi  +  m2  +  m3]  +  m4  L4 

[Lcj  '2 


ni22=  Icm2+  U 


m2 1 


cm2 


U 


+  1113 


2 

m33  =  Ic.m.3  +  Lcm.3  m3 

mi2  =  m2i  =  Li  L2  cos(02i) 


m2 


Lem. 2^ 
l2  j 


+  rri3 


mj3  =m3i  =  Li  UmaCos^jms 


(3.9) 

(3.10) 

(3.11) 

(3.12) 

(3.13) 

(3.14) 

(3.15) 
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m23=m32=  L2  Lcm.3  cos(e32)  m3 


(3.16) 


G(0,0) 


G(l) 

G(2) 

G(3) 


(3.17) 


G(1 )  =[  (m2  Li  Lc.m.2)  +  (m3  Lj  L2) ]  sin  (02i)  92  + 

.  2 

(m3Li 

Lc.m.  3)  sin  (03J)  03 

G(2)  =  -  [  (m2  Li  Lc.m.,)+  (m3L!  L2)]  sin  (02i)©i  + 

.  2 

(m3  L2  L 

c.m.  Jsin(031)03 


(3.18) 


(3.19) 


G(3)  =  -  (m3  L  i  Lc  m.3)  sin  (02i)  0?  - 

(m3  L2  Lc.m.3)  sin  (032)  02 

4.  Applied  Torques 

D'  Alembert's  principle  for  virtual  work  expression  was  used  to  determine 
the  expressions  for  Q  for  the  equations  of  motion.  As  written,  Q  is  a  vector 
containing  the  torques  applied  by  the  actuators  at  each  joint  in  terms  of  inertial 
coordinates.  At  this  point  it  will  be  beneficial  to  rewrite  the  Q  vector  in  terms  of 
local  coordinates.  The  virtual  work  applied  to  the  system  is  given  by  the 
following  equation. 

5W  =  ^  Q,  80jS  ^  5Wi  (3.21) 

i=l  i=l 

The  elements  of  Q  are  in  terms  of  torques  applied  to  inertial  coordinates.  Since 
each  actuator  applies  a  local  torque,  it  was  beneficial  to  rearrange  (3.21)  and  write 
Q  in  terms  of  the  local  torques  applied  by  the  individual  actuators.  The  virtual 
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work  described  in  equation  (3.21)  can  be  described  in  terms  of  these  local  torques 
as  follows. 


5Wi  =ui  60i  -U2  50i  (3.22) 

5W  2  =  U2802-U3562  (3.23) 

8W3  =  113862  (3.24) 

In  matrix  format  Q  can  be  written  in  terms  of  a  control  influence  matrix  B  and  the 
local  torque  vector,  m  as  written  in  (3.25). 

Q  =  Bu  =  [Qi,Q2,Q3f  (3.25) 

Qi  =  ui  - 112  (3.26) 

Q2  =  U2  -  u3  (3.27) 

Q3  =  u3  (3.28) 

where 

r  1  -i  01  fur 

B  =  0  1-1  U=  u2 

.  0  0  lJ  Lu3J 

C  LYAPUNOV  STABLE  CONTROLLER  DESIGN 


This  design  for  a  stable  non-linear  controller  is  based  upon  Lyapunov’s 
Stability  Theory.  This  theory  is  also  known  as  Lyapunov's  Second  or  Direct 
method.  This  theory  is  covered  in  greater  detail  in  [Refs.  1,  2,  14,  and  15].  To 
review  the  Lyapunov  Stability  Theory,  a  system  without  any  external  forces  or 
torques  is  assumed.  This  system  is  assumed  to  have  a  single  equilibrium  state.  For 
this  system,  a  positive  definite  function  is  assumed  to  be  an  exact  integral  of  the 
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system  under  some  idealization.  This  function  is  termed  a  Lyapunov  function  and 
is  selected  to  satisfy  the  requirements  that  it  is  zero  at  the  desired  equilibrium  and 
positive  everywhere  else.  This  function  may  be  represented  by  the  system  total 
energy  or  the  Hamiltonian  of  the  system  for  most  of  the  time  invariant  mechanical 
systems.  If  this  system  is  perturbed  from  its  equilibrium  state,  the  energy  state  is 
increased  to  some  positive  energy  level.  Depending  upon  the  nature  of  the 
Lyapunov  function,  one  of  the  following  conclusions  can  be  drawn. 

•  If  the  system  dynamics  dictate  that  the  initial  energy  of  the  system  does 
not  increase  with  time,  then  the  system  is  stable. 

•  If  the  energy  of  the  system  monotonically  decreases  with  time  for  all  initial 
conditions,  and  eventually  goes  to  zero,  then  the  system  is  asymptotically 
stable. 

•  If  the  energy  increases  for  any  initial  condition,  then  the  system  is  unstable. 

•  If  the  energy  measure  neither  increases,  nor  decreases  as  a  function  of  time 
then  no  conclusion  can  be  drawn. 

Despite  the  power  of  this  theory,  there  is  no  unified  process  to  find  candidate 
Lyapunov  functions  that  globally  satisfy  stability  requirements.  A  more  complete 
description  of  the  Lyapunov  stable  controller  design  for  flexible  structures  can  be 
found  in  [Refs.  1  and  2], 

1.  Lyapunov  Stable  Controller  With  Linear  Feedback 

For  this  application,  a  simple  non-linear  controller  with  linear  feedback  of 
position  and  velocity  information  is  the  primary  objective.  The  candidate 
Lyapunov  function  for  this  application  is  of  the  form 

U  =  E  +  f(80i,802,803)  (3.29) 
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where  E  is  defined  as  the  "work  energy"  of  the  Lyapunov  function  and  "f"  is  a 
positive  function  of  80i,  502,  and  503.  Note  that  "f  is  a  pseudo  potential  energy 
that  renders  the  Lyapunov  function  positive  definite  with  respect  to  the  new 
equilibrium  point.  In  addition,  80*  =  0*  >  0jf)  where  0*  is  a  constant  that  describe 
the  final  joint  angle. 

(01,  02,  03,  01,  02,  03)  f  =  (0.f,  02f,  03f,  o,  0,  0)  (3.30) 

The  Lyapunov  function  in  (3.30),  can  be  differentiated  to  obtain 

v=E+im&6> 

The  "work  energy  rate",  E,  of  (3.31)  can  be  directly  obtained  from  (3.22)  through 
(3.24)  and  the  "pseudo  energy  rate"  of  the  system  is  defined  in  (3.33). 


E  =  (m  -  U2)  50i  +  (U2  -  U3)  802  +  u3  503  (3.32) 


3 

"Pseudo  Energy  Rate"  =  Y 

i=l 


(3.33) 


Equations  (3.32)  and  (3.33)  can  be  substituted  into  (3.31)  to  form  equation  (3.34) 
which  can  be  further  simplified  to  equation  (3.35). 


U  =  (ui  -  U2)  50i  +  (U2  -  U3)  502  +  113  803 


(3.34) 


U  =  50 1  u  1  -  U2  + 


3f 


3(80,))  +  ®>2\u2- 113  +  g(g0j) 

3f  \ 


603  ( 


(3.35) 


U3  + 


3{503) 


Based  on  (3.35),  it  is  evident  that  a  function  can  be  selected  such  that  U  <  0, 
which  is  the  stability  condition  in  the  Lyapunov  sense.  Therefore,  the  control 
laws  are  chosen  to  satisfy  the  following  relations 
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U1  -  U2  +  3(86,) -"Sl' 861 

(3.36) 

“2 '  “3  +  3(8e2) =  'fo-  802 

(3.37) 

where, 

“3  +  3(863)  =  'g3’ 803 

(3.38) 

for 

giv  >  0>  g2v  >  0,  g3v  >  0,  glp  >  0,  g2p  >  0,  and  g3p  >  0 

f>0 

In  addition. 

U  =  -(fci  U01  +  g2,  502  +  g3,  503  )  (3.40) 

By  selecting  an  appropriate  function  for  "f  equation,  the  stabilizing  control  laws 

(ui,  u2,  u3)  satisfying  the  U  <  0  requirement  can  be  built.  For  this  case,  assume 

that  "f '  is  defined  as  follows 

f  ^  (g  iP  50?  +  g2p  502  +  g3p  503) 

(3.41) 

where 


gip  >  0,  g2p  >  0,  g3p  >  0 

For  a  controller  using  pure  linear  feedback,  (3.36-3.38)  can  be  simplified  as 
described  below. 

(3.42) 

(3.43) 


^3  =  "  g3p  503  “  g3v  S©3 
U2=U3-g2p  502^2,  502 

Ui=U2-gip80i-giv50i 
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(3.44) 


where 


58j  =  0,  -  0if ,  for  i  =  1 , 2, 3 
58i  =  0j  -  0if 


(3.45) 


(3.46) 


0*  =  0  for  i  =  1 , 2, 3 

2.  Lyapunov  Stable  Tracking  Controller 

In  a  similar  way,  the  Lyapunov  stable  tracking  controller  can  also  be 
derived.  Lyapunov  stability  is  not  proven  here  but  is  discussed  in  more  detail  in 
[Ref.  1].  In  this  application,  a  function  generator  was  used  to  generate  a  reference 
trajectory  for  the  controller  to  follow.  With  this  type  of  controller,  a  fifth  order 
polynomial  was  used  to  generate  a  desired  tip  trajectory  with  a  two  link  closed 
loop  inverse  kinematic  solution  in  one  case.  In  another  case  a  "near-minimum¬ 
time"  torque  shaping  scheme  was  utilized  to  generate  a  reference  trajectory.  Both 
these  trajectory  generators  will  be  discussed  in  more  detail  in  following  sections. 
The  control  torques  required  for  this  reference  tracking  controller  are  presented 
next. 


where 


8u3=-g^503-g3, 803 

(3.47) 

§U2  =  Su3  -  g2p  502  -  g2v  802 

(3.48) 

5uj  =  Su2  -  gip  50i  -  giv  501 

(3.49) 

50i  =  0i  -  0iref 

(3.50) 

80i  =  ©i  -  0^ 

(3.51) 

6uj  =  ui  -  u* 

(3.52) 
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The  control  laws  presented  above  can  be  rigorously  proven  to  satisfy  the 
Lyapunov  Stability  condition  if  50j  and  §0*  are  very  small. 

D.  REFERENCE  TRAJECTORY  GENERATION 

For  the  tracking  controller  developed  previously  while  discussing  the 
Lyapunov  stable  tracking  controller  requires  some  reference  trajectories.  Here 
trajectory  refers  to  a  time  history  of  position,  velocity,  and  acceleration  for  each 
degree  of  freedom.  The  primary  consideration  for  generating  a  trajectory  lies  in 
the  fact  that  the  trajectory  must  first  of  all  be  smooth  and  second  it  must  be  easily 
calculated.  In  this  thesis,  two  different  techniques  will  be  used  to  generate  the 
required  trajectory  information  to  stabilize  and  control  the  system. 

The  first  trajectory  that  will  be  discussed  involves  using  a  fifth  order 
polynomial  to  describe  the  path  of  the  tip  of  the  link  three  of  the  manipulator.  In 
the  second  trajectory,  a  near-minimum-time  maneuver  using  input  torque  shaping 
will  be  used  to  generate  the  desired  trajectory. 

1.  Polynomial  Reference  Trajectory 

For  the  tracking  controller  discussed  in  the  previous  section,  it  is  usually 
difficult  if  not  impossible  to  obtain  the  open-loop  solutions  for  the  theoretical 
reference  system  of  differential  equations.  For  practical  considerations  it  is  often 
advantageous  to  design  the  control  system  that  will  follow  an  easy  to  calculate 
path.  For  robotic  applications,  a  polynomial  of  order  3  or  higher  is  often  used  to 
specify  the  position  of  the  end  of  the  manipulator.  For  this  application,  imagine  a 
two  link  manipulator  attached  to  the  spacecraft  main  body  as  depicted  in  Figure 
3.3.  In  this  case,  the  maneuver  attempts  to  position  link  two  from  an  initial  angle 
of  20  degrees  to  a  final  position  of  40  degrees  and  link  three  is  maneuvered  from 
40  to  60  degrees.  For  a  two  link  manipulator,  there  exists  a  closed  loop  solution 
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to  the  inverse  kinematic  problem  that  can  be  quickly  and  easily  computed.  The 
closed  loop  solution  is  described  in  more  detail  in  appendix  B.  For  this 
manipulator,  given  the  beginning  and  final  coordinates  of  the  manipulator,  the 
vector  r  is  used  to  specify  the  position  of  the  end  point  of  link  two  as  a  function 
of  time.  This  vector  is  presented  in  Figure  3.3. 


Figure  3.3  Polynomial  Reference  Maneuver 


26 


r  (t)  =  r  (to)  +  f  (t)  [r  (tf)  -  r  (to)]  (3.53) 

To  simplify  the  calculations,  t  can  be  written  as  a  function  of  normalized  time,  X, 
given  the  start  time  to  and  finish  time  tf.  Note  that  for  to  <  t  <  tf,  then  0  <  X  <  1 . 


X  =  ^-k  (3.54) 

tf-  to 

A  fifth  order  polynomial  was  used  for  this  application.  This  allowed  the 
user  to  specify  the  beginning  and  final  velocities  and  accelerations  as  well  as  the 
beginning  and  final  positions  of  the  tip.  The  polynomial  for  this  controller  was  of 
the  form 

f(x)  =  Cl  +  C2  x  +  C3  X2  +  C4  X3  +  C5  X4  +  C6  X5  (3.55) 

and  was  subject  to  the  following  boundary  conditions. 

f  (0)  =  0,f  (0)  =  O.f  (0)  =  0 

f(l)=  l.f  (1)  =  O.f  (1)  =  0 
The  resulting  expression  for  f(X)  is  obtained  as 

f(x)  =  10  x3  - 15  x4  +  6  x5  (3.56) 

Equation  (3.56)  can  be  differentiated  with  respect  to  time.  These 
expressions  were  utilized  to  calculate  velocities  and  accelerations  and  are 
presented  in  equations  (3.57)  and  (3.58). 

f(x)  =  30  x2  -  60  x3  +  30  x4  (3.57) 

f(x)  =  60  x-  180  x2+  120  x3  (3.58) 

Equation  (3.53)  can  also  be  differential  with  respect  to  time  to  give  the  velocity 
and  acceleration  of  the  tip  of  link  three  purely  as  a  function  of  time. 

Vector  Position: 


(3.59) 


r(x)  =  r(to)+f(x)[r(tf)- r(t0)] 

Vector  Velocity: 

r  (x)  =  f  (T) 

Vector  Acceleration: 

r(r)  =  f  (t) 

With  the  position  of  the  tip  known  as  a  function  of  time,  the  angles  02i 
and  032  can  be  solved  for  directly  as  can  02  and  03  via  the  two  link  closed  loop 
solution  of  the  inverse  kinematics  problem.  At  this  point  0i,  0i,  and  0j  are  all 
assumed  to  be  comparatively  small.  The  Jacobian  can  be  used  to  define  the 
relationship  between  the  velocity  and  angular  velocity  as  well  as  the  tip 
acceleration  and  angular  acceleration  as  described  in  the  following  equations. 


tf  -  to 


M-1M' 


.  -  to  f  J 


(3.60) 


(3.61) 


?=H|e) 

where  H  is  the  Jacobian  corresponding  to  the  given  configuration. 
[  - 12  sin  (02),  -  I3  sin  (03) 

H  — 

12  cos  (02),  13  cos  (03) 


(3.62) 


(3.63) 


r  =  H  (0)  +  H  (0}  (3.64) 

•  f  - 12  cos  (02),  - 13  cos  (03)  1 

ti  =  (3.00) 

-  12  sin  (02),  -  I3  sin  (@3) 

With  the  above  equations,  he  position  (angular  and  cartesian),  velocity 
(angular  and  cartesian),  and  acceleration  (angular  and  cartesian),  can  all  be 
determined  as  functions  of  time.  With  this  information,  and  the  physical 
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characteristics  of  the  system,  the  reference  torques  can  also  be  determined  using 
equation  presented  below. 

uref = b1  [m  (e)  e  +  G(e,e)]  (3.66) 

2.  Minimum-Time-Maneuver 

For  an  ideal  case,  of  a  single  degree  of  freedom  system,  the  minimum  time 
required  to  perform  a  particular  maneuver  is  achieved  by  applying  the  maximum 
available  torque  for  one-half  of  the  time  required  to  complete  the  maneuver 
followed  by  the  remaining  half  with  the  maximum  negative  torque.  This  results  in 
a  controller  where  the  torque  is  always  operated  at  its  maximum  value  and  gives 
rise  to  a  torque  shaping  function,  position,  velocity,  and  accelerations  profiles  as 
presented  in  Figure  3.4.  This  type  of  controller  is  sometimes  referred  to  as  a  bang- 
bang  controller. 


Figure  3.4  Bang-Bang  Minimum  Time  References 


For  an  application  with  a  single  degree  of  freedom,  the  relationship 
between  the  angular  acceleration  and  the  applied  torque  is  given  by  equation 
(3.67)  with  the  applied  torques  given  by  equation  (3.68). 


I  0  =  u 


(3.67) 


u  = 


Umax,  for  0  <  t  <  £ 

2 

-Umax,  2  ~  t  -  tf ' 


(3.68) 


\  0,  for  tf  <  t  | 

Bang-Bang  control  theory  for  minimum  time  maneuvers  is  discussed  in  more 
detail  in  [Refs.  1  and  2]. 

3.  Near-Minimum-Time  Rigid  Body  Maneuver 

A  more  general  case  of  the  bang-bang,  minimum  time  controller  is  the 
near-minimum-time  controller.  The  bang-bang  controller  does  maneuver  the 
manipulator  from  the  initial  to  final  positions  in  the  minimum  amount  of  time.  The 
primary  drawback  of  the  bang-bang  controller  is  the  rapid  rise  of  the  torque 
trajectory.  This  results  in  a  rapid  acceleration  followed  by  a  rapid  deceleration 
which  will  require  actuators  with  instantaneous  switching  capacity  which  is  not 
practical  and  a  robust  structural  design  for  the  manipulators  themselves.  By 
introducing  an  input  shape  function,  the  instantaneous  rise  in  the  torque 
trajectory  can  be  reduced,  resulting  in  slightly  smaller  accelerations  which  will  in 
turn  require  smaller  actuators  and  reduced  structural  requirements.  For 
comaparison,  the  input  shaping  function  for  a  =  0.1  and  a  =  0.25  are  presented 
in  Figures  3.5  and  3.6  respectively. 

The  differential  equations  used  to  describe  the  minimum  time  maneuver 
above  can  be  modified  so  that  they  are  of  the  form. 


I  0ref  —  tlref  —  tlmax  f(At,tf,t) 


(3.70) 


30 


Normalized  Amplitude  I  I  Normalized  Amplitude 


Figure  3.5  Normalized  Input  Shaping  Funciton  With  a  = 
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Where  the  input  shaping  function  is  suggested  in  the  form. 


=feF[3-2(ilfor0£t<At 

=  1  for  At  <  t  <  ti 


fK,  2{(^f 


1  for  ti  <  t  <  t2  \ 

W  2At  1 

\  2At  \\ 

=  -  1  for  t2  ^  t  <  t3 


=  -  1  + 


fort3St<“ 


(3.69) 


where 


tf  =  Maneuver  Time 
ti  =ts- At 

t2  =  ts  +  At 

t3  =  tf  -  At 

At  =  at{  =  Rise  Time 

ts  =  P  tf  =  Switching  Time,  where  (3  =  0.5 
To  determine  a  relationship  between  the  angular  acceleration  and  the 
maximum  available  torque  we  begin  with  the  non-linear  equations  of  motion 

M(e)  e  +  G(e,  e)  =  b  u  @.70) 

where 

M(0)  -  Mass  Matrix 

gM)  -  Coriolis  Acceleration  Terms 

u  -  Local  Torques 
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Equation  (3.70)  is  linearized  by  eliminating  all  non-linear  cosine,  sine,  and  higher 
order  terms.  These  linearization  process  results  in  the  following  equation  for  an 
i*  link. 

I  ©ref  =  uref  =  umax  f(At,tf,t)  (3.71) 


I  -  Moment  Of  Inertia  About  Axis  Of  Rotation 
Equation  (3.71)  can  be  reorganized  into  the  form  of  the  following  equation. 


=  ^max  f(At,tf,t) 


(3.72) 


In  this  equation,  Umax  is  a  design  parameter  that  is  determined  by  the 
characteristics  of  the  actuator  used  to  drive  the  manipulators.  For  this  application, 
all  the  actuators  are  the  same  geared  motor.  f(At,tf,t)  can  be  modified  by  varying 
the  a  variable.  I,  the  linearized  mass  moment  of  inertia,  is  determined  by  the  mass 
characteristics  of  the  system  and  does  not  vary  with  time  or  changing  geometry. 
Given  a  maximum  torque  umax.  and  an  input  shaping  function  with  suitable 
boundary  conditions,  the  reference  position  and  velocities  can  be  determined  by 
integrating  the  following  equations  piece  wise  of  the  time  interval  determined  by 
the  maximum  torque. 


QreKO  -  ®0  + 


^  I  f(At,tf,x)  dx 

J  k> 


(3.73) 


eref(t)  =  e0 + e0  (t-to)  + 


({ 


f(At,tf,x2)  dx2  dxi  (3.74) 
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where 


to  =  0  0(0)  =  ©o  6(0)  =  0 

t  =  tf  0  (tf)  =  0f  0(tf)  =  0 


are  the  boundary  conditions  at  the  start  and  completion  of  the  maneuver.  The 
resulting  reference  equations  are  presented  in  Appendix  B. 

The  near-minimum-time  to  complete  the  maneuver  can  be  obtained  by 
setting  t  =  tf  in  the  equation  presented  below  and  obtained  from  Appendix  B. 


0f  = 


_  u 


max 


J--  J-OC  + 
14  2 


10 


a" 


tr2 


(3.75) 


By  doing  this,  a  relationship  between  the  time  to  complete  the  maneuver 
and  the  maximum  torque  available  can  be  determined  for  an  i^  link. 


ti  = 


Umaxj  — 


ii(ef,-eb) 


a“l4'2“+Wrf) 


(3.76) 


(3.77) 


\4  2  "  10 

For  this  application,  the  time  derived  with  the  above  equations  represents  the 
near-minimum-time  required  to  perform  the  maneuver  for  only  one  joint. 


E.  ANALYTICAL  MODEL  SUMMARY 

This  research  project  analyzes  how  a  two  link  manipulator  can  be  controlled 
from  a  spacecraft  main  body  while  minimizing  the  effect  of  manipulator  motion 
upon  the  main  body  In  general  one  can  calculate  the  interaction  force  between 
the  links  and  main  body  by  using  the  Newtonian  approach  with  free  body 
diagrams.  Based  on  this  analysis,  the  more  smoothly  that  the  maneuver  is 
performed,  the  smaller  the  interactive  force  will  be.  Two  basic  type  of  controllers 
were  developed.  In  the  first  controller,  linear  feedback  of  position  and  velocity 
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information  from  the  joints  was  used  to  control  the  endpoint  position.  In  the 
second  type  of  controller,  position  and  velocity  information  are  used  in 
conjunction  with  a  reference  trajectory  to  control  endpoint  position. 

For  this  type  of  controller,  two  different  schemes  were  used  to  generate  the 
reference  trajectory.  In  the  first,  a  fifth  order  polynomial  in  conjunction  with  the 
two  link  closed  loop  solution  for  the  manipulator  inverse  kinematics  and  the 
Jacobian  were  used  to  generate  the  reference  trajectory.  In  the  second,  a  near¬ 
minimum-time  torque  shaping  technique  was  used  to  determine  the  reference 
trajectory.  All  components,  including  the  manipulator  and  main  body,  are 
modeled  as  rigid  bodies. 
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IV.  RESULTS  AND  DISCUSSION 


Analytical  results  from  the  simulation  are  presented  in  this  section.  Three 
different  techniques  were  used  to  control  and  stabilize  the  position  of  the 
manipulator.  Results  from  the  linear  feedback  controller  with  constant  gains, 
polynomial  reference  tracking  controller  and  near-minimum-time  reference 
tracking  controllers  are  presented  sequentially. 

A.  REFERENCE  MANEUVER 

The  three  most  import  criteria  with  respect  to  this  research  are  the  end  tip  final 
position,  joint  torques,  and  the  effect  of  manipulator  motion  on  the  attitude  of  the 
main  body.  Final  position  for  the  endpoint  of  the  manipulator  is  critical  for 
performing  assembly  tasks.  Joint  torques  are  important  to  verify  if  the  servo- 
actuators  can  perform  the  desired  maneuvers.  The  effect  of  the  manipulator 
motion  upon  the  main  body  is  critical  due  to  the  excitation  of  flexible  structures 
on  the  spacecraft  or  the  effect  upon  antenna  pointing  accuracy  to  maintain  a 
communications  link. 

To  be  able  to  effectively  compare  the  performance  of  the  three  different 
controllers,  a  standard  reference  maneuver  was  developed.  For  this  maneuver,  the 
desired  rotation  of  the  main  body  was  zero.  Link  1  was  programmed  to  move 
from  an  inertial  angle  of  20  degrees  to  40  degrees.  Link  2  was  programmed  to 
move  from  an  initial  inertial  angle  of  40  degrees  to  a  final  position  of  60  degrees. 
This  reference  maneuver  is  depicted  in  Figure  4.1 
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Figure  4.1  Reference  Maneuver 

B.  LINEAR  CONSTANT  GAIN  FEEDBACK  CONTROLLER 

The  theory  behind  the  linear  constant  gain  feedback,  controller  is  described 
in  more  detail  in  Chapter  III  and  [Ref.  1,15].  Position  gains  of  1  (  Gp  =  1  )and 
velocity  gains  of  5  (  Gv  =  5  )  were  used  for  these  plots.  Small  gain  (  Gp  =  0. 1  and 
Gy  =  0.2 )  plots  are  presented  in  Appendix  D  for  comparison. 

The  linear  feedback  controller  was  the  simplest  controller  conceptually  and 
the  easiest  to  implement.  This  controller  provided  stable  control  over  a  wide 
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range  of  gains  and  for  a  wide  variety  of  maneuvers.  No  knowledge  of  the  system 
mass  or  inertia  characteristics  was  required  for  this  controller.  Only  the  position 
and  velocity  gains,  and  beginning  and  final  arm  positions  were  required  to 
calculate  the  required  control  torques  to  perform  the  maneuver. 

The  joint  time  history  for  this  case  is  presented  in  Figure  4.2  and  the  joint 
velocity  time  history  is  presented  in  Figure  4.3.  Performance  of  this  control 
system  was  directly  related  to  gain  selection.  The  system  was  relatively  sensitive 
to  small  changes  in  the  position  and  velocity  gains.  This  resulted  in  changes  to 
the  maneuver  time  and  control  system  damping.  The  system  was  stable  for  all 
gains  and  maneuvers  evaluated  and  it  could  be  proven  by  Lyapunov  stability 
theory  that  the  system  was  globally  stable.  Even  with  system  stability  not  really 
in  question,  there  was  no  way  to  systematically  select  position  and  velocity  gains 
to  optimize  maneuver  time  or  torques  to  achieve  desired  performance  measures. 

The  magnitude  of  the  gains  directly  impacted  the  performance  of  the 
manipulator  and  the  stability  of  the  main  body.  Larger  gains  generated  larger 
control  torques  and  reduced  the  time  required  to  perform  the  maneuver.  The 
damping  of  the  system  could  also  be  varied  with  the  gain  selection.  The  effect  of 
this  movement  on  the  main  body  was  small  but  still  resulted  in  a  displacement  of 
nearly  2  degrees.  Although  this  is  apparently  a  small  displacement,  2  degrees  may 
still  cause  degradation  in  communication  with  the  satellite  due  to  antenna 
pointing  errors. 

Time  history  plots  for  torques  and  momentum  wheel  speed  are  presented  in 
Figures  4.4  and  4.5  respectively.  The  response  of  this  system  to  the  controller 
was  characterized  by  a  very  rapid  increase  in  torque  immediately  after  initiation  of 


38 


Joint  Velocity  (Deg/Sec)  n  Joint  DisPlacement  (D*«) 


e  4.2  Linear  Constant  Gain  Feedback  Joint  Position  Time  History  With 

Gp=l  and  Gy=5 


Figure  4.3  Linear  Constant  Gain  Feedback  Joint  Velocity  Time  History  With 

Gp=l  and  Gy— 5 
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Wheel  Speed  (RPM) 


Figure  4.5  Linear  Constant  Gain  Feedback  Momentum  Wheel  Speed  Time 

History  With  Gp=l  and  Gv=5 
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the  maneuver  followed  by  a  rapid  decrease  after  a  few  seconds.  This  response 
was  similar  to  an  impulse  input  that  became  more  pronounced  with  increased 
commanded  motion  or  with  increased  gains.  Large  gains  are  desirable  to  achieve 
acceptable  levels  of  performance  but  must  be  balanced  against  actuator 
characteristics  ,  magnitude  of  the  maneuver,  and  structural  considerations..  A 
large  and  sudden  increase  in  commanded  torque  could  saturate  the  actuator  or 
excite  flexible  appendages  attached  to  the  spacecraft. 

The  large  initial  torques,  depicted  in  Figures  4.4  and  4.5,  correspond  to 
rapid  changes  in  the  speed  of  the  momentum  wheel.  Large  speed  changes  are 
required  during  the  initial  portion  of  the  maneuver  to  generate  large  toques  to 
counteract  the  forces  generated  by  the  manipulator.  Based  on  previous 
experience  with  the  FSS  and  analysis  of  electrical  power  requirements  of  the 
actuator  and  power  supply,  it  is  reasonable  that  the  actuator  will  be  able  to 
compensate  for  the  torques  generated  by  the  movement  of  the  manipulator. 

C  POLYNOMIAL  REFERENCE  TRACKING  CONTROLLER 

To  rectify  the  problems  encountered  with  the  simple  linear  constant  gain 
feedback  controller  a  reference  tracking  controller  was  implemented.  In  the 
previous  case,  there  was  a  large  initial  error  for  which  the  system  was  attempting 
to  correct.  In  this  controller,  a  smooth,  fifth  order  polynomial  was  used  to 
generate  a  reference  trajectory  so  that  the  correction  could  be  evenly  partitioned 
over  a  specified  maneuver  time.  With  this  information,  reference  torques,  angular 
position,  velocity,  and  acceleration  were  obtained  as  a  smooth  function  of  time. 
As  a  result  the  initial  "jump"  generated  by  the  linear  feedback  controller  was 
eliminated  and  the  manipulator  was  able  to  track  a  smooth  path  from  initial  to 
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final  conditions.  A  variation  of  the  Lyapunov  stable  control  law  used  in  the 
previous  controller  was  used  with  this  reference  trajectory  controller. 

With  this  controller,  the  time  required  to  perform  the  maneuver  could  be 
specified  but  was  subject  to  the  characteristics  of  the  actuators  and  the  task  being 
performed.  Two  maneuver  times  were  analyzed.  In  the  first  case,  the  time  to 
perform  the  maneuver  was  fixed  at  2.5  seconds.  This  time  was  selected  to 
determine  joint  torques,  velocities,  and  momentum  wheel  speeds  while  operating 
near  the  torque  limit  of  the  actuators.  In  the  second  case,  a  time  of  5  seconds 
used  to  determine  the  effect  of  maneuver  time  on  the  actuators.  These  plots  for 
the  5  second  maneuver  time  are  included  in  Appendix  D  to  demonstrate  the  effect 
of  maneuver  time  upon  joint  torques  and  velocities. 

Position  and  velocity  time  history  of  the  manipulator  for  a  maneuver  time  of 
2.5  seconds  are  presented  in  Figure  4.6.  and  4.7  respectively.  Torque  and 
momentum  wheel  speed  plots  for  a  maneuver  time  of  2.5  seconds  are  presented  in 
Figures  4.8  and  4.9  respectively. 

Movement  of  the  manipulator  did  not  cause  measurable  disturbance  to  the 
main  body.  Manipulator  torques  were  smoothly  applied,  and  the  control  system 
was  able  to  counter  these  torques  effectively  and  in  a  timely  manner,  negating 
motion  by  the  main  body.  The  stability  of  the  main  body  during  manipulator 
motion  is  desirable  since  it  will  reduce  pointing  errors  for  unstablized  imaging 
devices  or  high  gain  antenna  communications  systems. 

The  polynomial  reference  controller  demonstrated  stable  operation  over  a 
wide  variety  of  operating  conditions  and  feedback  gains.  The  magnitude  of  the 
gains  had  little  or  no  effect  upon  the  maneuver  time  since  this  characteristic  was 
specified  in  the  reference  maneuver.  Commanded  torques  closely  tracked  the 


42 


Angular  Velocity  (Dcg/Scc)  |  70*  j  Angular  Displacement  (Deg) 


e  4.6  Polynomial  Reference  Tracking  Controller  Joint  Position  Time 
History  With  Gp=l,  Gv=5  And  Maneuver  Time  Of  2.5  Seconds 


Figure  4.7  Polynomial  Reference  Tracking  Controller  Joint  Velocity  Time 
History  With  Gp=l,  G\=5  And  Maneuver  Time  Of  2.5  Seconds 
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Figure  4.8  Polynomial  Reference  Tracking  Controller  Torque  Time  History 
With  Gp-1,  Gy=5  And  Maneuver  Time  Of  2.5  Seconds 


Figure  4.9  Polynomial  Reference  Tracking  Controller  Momentum  Wheel 
Speed  Time  History  With  Gp=l,  Gy=5  And  Maneuver  Time  Of  2.5  Seconds 
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reference  torques  with  no  discernible  difference  between  the  reference  and 
commanded  torques.  The  polynomial  reference  tracking  controller  provided 
smooth  position  control  commands  for  all  maneuvers.  With  the  tracking 
controller,  there  were  no  discontinuities  or  sudden  changes  in  torque  commands. 
The  performance  was  independent  of  the  magnitude  of  the  commanded  maneuver 
with  no  effect  of  gain  selection  being  noted.  Maneuver  time  could  be  specified 
but  was  subject  to  actuator  and  structural  characteristics.  Feedback  errors 
remained  small  throughout  the  maneuver  as  the  controller  smoothly  tracked  the 
reference.  This  resulted  in  smoothly  changing  torque  commands  with  smooth  and 
predictable  motion  of  the  manipulator. 

D.  NEAR-MINIMUM-TIME  REFERENCE  TRACKING  CONTROLLER 

The  theory  behind  the  near-minimum-time  tracking  controller  is  described  in 
Chapter  III  and  [Ref.  1,2].  Two  cases  for  this  controller  were  selected.  In  the  first 
case,  a  was  set  equal  to  0.25.  The  shape  function  generated  by  this  reference  are 
sinusoidal  in  shape.  In  the  second  case,  an  a  of  0.1  was  used.  As  a  approaches 
zero,  the  input  torque  shape  approached  that  of  a  square  wave  with  a  period  of 
the  maneuver  time.  This  shape  more  closely  approximated  the  minimum  time 
bang-bang  controller.  A  maneuver  times  of  2.5  seconds  was  used  so  that  the 
polynomial  reference  and  the  near-minimum-time  control  system  performance 
characteristics  could  be  compared.  Similar  plots  with  a  maneuver  time  of  5 
seconds  can  be  found  in  Appendix  D. 

The  general  performance  of  the  near-minimum-time  reference  tracking 
controller  provided  some  of  the  advantages  and  disadvantages  of  the  two 
previous  control  systems.  Since  this  was  a  tracking  controller  and  the  feedback 
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errors  were  generally  small,  the  commanded  torques  were  generally  smooth  with 
only  minor  disturbances  to  the  main  body. 

Near  minimum-time  reference  position  and  velocity  time  history  of  the 
manipulator  for  a  maneuver  time  of  2.5  seconds  with  a  =  0.25  are  presented  in 
Figure  4.10.  and  4.1 1  respectively.  Momentum  wheel  speed  plots  for  a  maneuver 
time  of  2.5  seconds  with  a  =  0.25  are  presented  in  Figures  4.12  and  4.13 
respectively.  Near  minimum-time  reference  position  and  velocity  time  history  of 
the  manipulator  for  a  maneuver  time  of  2.5  seconds  with  a  =  0.1  are  presented  in 
Figure  4.14.  and  4.15  respectively.  Momentum  wheel  speed  plots  for  a  maneuver 
time  of  2.5  seconds  with  a,  =  0.1  are  presented  in  Figures  4.16  and  4.17 
respectively. 

With  a  =  0.25  the  input  torque  shaping  closely  resembles  that  of  the 
polynomial  reference  trajectory  tracking  controller.  As  with  the  polynomial 
reference  trajectory,  the  manipulator  links  moved  smoothly  from  the  initial  to  the 
final  conditions.  Note  that  for  this  controller,  there  is  only  a  very  small  angular 
displacement  of  the  main  body. 

Joint  velocities  for  the  manipulator  links  follow  a  smooth  path,  beginning  and 
ending  with  zero  velocities.  With  this  controller,  note  that  unlike  the  previous 
controller,  there  is  no  phase  difference  between  the  movement  of  the  manipulator 
links.  Both  links  move  with  the  same  velocity  at  the  same  time.  The  main  body 
also  displays  a  very  small  angular  velocity. 
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Time  (Sec) 


Figure  4.10  Near-Minimum  Time  Tracking  Controller  Joint  Position  Time 
History  With  Gp=l  and  Gv=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.25 


Figure  4.11  Near-Minimum  Time  Tracking  Controller  Joint  Velocity  Time 
History  With  Gp=l  and  Gy=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.25. 
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Figure  4.12  Near-Minimum  Time  Tracking  Controller  Torque  Time  History 
With  Gp=l  and  Gv=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.25 


Figure  4.13  Near-Minimum  Time  Tracking  Controller  Wheel  Speed  Time 
History  With  Gp=l  and  Gy-5,  Maneuver  Time  Of  2.5  Seconds  ,  And  a  =  0.25 
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Figure  4.14  Near-Minimum  Time  Tracking  Controller  Joint  Position  Time 
History  With  Gp=l  and  Gv=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.1 


Figure  4.15  Near-Minimum  Time  Tracking  Controller  Joint  Velocity  Time 
History  With  Gp=l  and  Gv=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.1 
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Figure  4.16  Near-Minimum  Time  Tracking  Controller  Torque  Time  History 
With  Gp=l  and  Gy=5,  Maneuver  Time  Of  2.5  Seconds  ,  And  a  =  0.1 


Figure  4.17  Near-Minimum  Time  Tracking  Controller  Wheel  Speed  Time 
History  With  Gp=l  and  Gv=5,  Maneuver  Time  Of  2.5  Seconds ,  And  a  =  0.1 
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As  a  goes  toward  zero,  the  shape  of  the  input  torque  reference  becomes 
more  square.  The  square  comers  of  the  torque  input  are  difficult  for  the  controller 
to  track  and  required  larger  gains.  This  characteristic  is  best  noted  in  the  previous 
depicted  plots  with  a  =0.1.  Note  that  rapid  changes  in  the  wheel  speed  are 
required.  Even  with  large  gains,  the  main  body  was  still  disturbed  by  motions. 
The  jerky  motion  of  the  manipulator  disturbed  the  main  body  and  resulted  in  small 
but  detectable  position  changes.  As  in  the  previous  controllers,  any  disturbance 
to  the  main  body  can  degrade  communications  links  or  cause  pointing  errors  for 
optical  imaging  devices. 
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V.  CONCLUSIONS 


Three  different  control  systems  were  simulated  and  analyzed.  All  three 
controllers  were  stable  and  were  able  to  position  the  manipulator  in  a  timely  and 
effective  manner.  The  most  significant  difference  between  the  controllers  turned 
out  to  be  how  effective  the  control  system  was  stabilizing  the  main  body  while 
positioning  the  manipulator. 

The  linear  feedback  controller  was  the  simplest  controller  conceptually  and 
the  easiest  to  implement.  This  controller  provided  stable  control  over  a  wide 
range  of  gains  and  for  a  wide  variety  of  maneuvers.  Large  gains  were  required  to 
achieve  acceptable  levels  of  performance.  Large  gains  corresponded  to  large 
torques  and  large  displacements  of  the  main  body.  Position  and  velocity  gains 
had  to  be  selected  to  balance  control  torques,  system  performance,  and  motion  of 
the  main  body.  As  the  control  system  was  implemented,  it  was  not  possible  to 
achieve  all  of  these  objectives  for  the  given  reference  maneuver. 

The  polynomial  reference  trajectory  for  robotic  applications  represents  a 
classic  approach  for  generating  a  reference  trajectory  to  position  and  control  a 
two  link  manipulator.  This  approach  also  offers  the  advantage  that  the  reference 
trajectory  can  be  pre-calculated  off  line  prior  to  the  maneuver.  These  pre¬ 
calculated  values  for  the  reference  trajectory  can  be  used  in  conjunction  with  the 
controller  to  minimize  real  time  computational  requirements. 

The  polynomial  reference  tracking  controller  provided  smooth  position 
control  commands  for  all  maneuvers.  With  the  tracking  controller,  there  were  no 
discontinuities  or  sudden  changes  in  torque  commands.  The  performance  was 
independent  of  the  magnitude  of  the  commanded  maneuver  and  only  slightly 
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affected  by  gain  selection.  Maneuver  time  could  be  specified  but  was  subject  to 
actuator  and  structural  characteristics.  Feedback  errors  remained  small 
throughout  the  maneuver  as  the  controller  smoothly  tracked  the  reference.  This 
resulted  in  smoothly  changing  torque  commands  with  smooth  and  predictable 
motion  of  the  manipulator.  The  polynomial  reference  tracking  controller  was  the 
most  effective  controller  implemented  with  respect  to  accurate  and  timely 
positioning  of  the  manipulator,  and  stabilization  of  the  main  spacecraft  body. 

The  general  performance  of  the  near-minimum-time  reference  tracking 
controller  provided  some  of  the  advantages  and  some  of  the  disadvantages  of  the 
two  previous  control  systems.  Since  this  was  a  tracking  controller  and  the 
feedback  errors  were  generally  small,  the  commanded  torques  were  generally 
smooth  with  only  minor  disturbances  to  the  main  body. 

The  near- minimum- time  for  the  reference  maneuver  was  determined  by  the 
maximum  torque  capability  of  each  actuator  and  the  magnitude  of  the  maneuver. 
The  maximum  of  the  three  times  calculated  for  each  of  the  three  joints  was 
specified  as  the  maneuver  time.  For  each  maneuver,  one  joint  would  then  become 
the  limiting  case  for  the  selected  maneuver  based  on  these  requirements.  Two  of 
the  three  remaining  actuators  therefore  would  operate  at  less  than  the  maximum 
toque  capability  in  order  to  complete  the  maneuver  at  the  same  time  as  the  other 
links  completed  the  maneuver. 

Large  gains  were  required  to  force  the  controller  to  closely  track  the 
reference  input  as  the  input  torque  shape  approached  the  minimum  maneuver 
time.  Even  with  larger  gains,  the  controller  was  not  able  to  completely  negate  the 
torques  generated  by  the  manipulator  and  some  small  rotations  of  the  main  body 
were  observed. 
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Two  different  control  laws  were  evaluated.  In  the  first,  constant  gains  with 
linear  feedback  was  utilized.  This  controller  proved  to  be  the  easiest  to  implement 
but  did  not  effectively  stabilize  the  main  body  during  movements  by  the 
manipulator.  The  second  control  law  entailed  using  a  tracking  controller  with 
constant  gains.  The  reference  tracking  controller  represents  a  traditional  and 
successful  way  of  positioning  the  manipulator.  With  the  reference  tracking 
controller  two  different  references  were  used.  In  the  first  a  polynomial  was  used 
to  generate  a  reference  trajectory.  This  technique  proved  to  be  only  slightly  more 
complex  than  the  linear  feedback  controller  with  constant  gains,  was  dependent 
upon  mass  and  inertia  characteristics  of  the  manipulator  and  was  the  most 
effective  at  positioning  the  manipulator  while  minimizing  the  motion  of  the  main 
body.  In  the  second,  a  near-minimum-time  technique  was  employed  to  generate  a 
reference  trajectory.  This  reference  was  more  complex,  but  provided  the 
capability  to  position  the  manipulator  in  near-minimum-time.  This  technique 
provided  greater  flexibility  in  shaping  the  input  torque  reference  but  was  not  as 
successful  as  the  polynomial  reference  tracking  controller  in  stabilizing  the  main 
body.  In  the  end,  the  polynomial  reference  tracking  controller  provided  the  best 
performance  of  the  three  controllers  simulated. 

A.  SUBJECTS  FOR  FUTURE  RESEARCH 

Lyapunov  theory  represents  only  one  methodology  for  nonlinear  control 
system  design.  Many  other  methodologies  including  neural  networks,  adaptive 
controllers,  sliding  controllers,  and  robust  controllers  among  others  provide 
additional  areas  for  research. 

With  the  addition  of  the  vision  server  to  the  Spacecraft  Robotics  Simulation 
Lab,  the  capability  to  track  a  target  as  well  as  the  endpoint  of  the  manipulator  will 
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become  available.  This  provides  the  capabilty  to  perform  end  point  tracking 
tasks.  Once  this  is  successful  with  a  stationary  target,  the  same  can  be  repeated 
with  a  moving  target. 

The  design  of  the  manipulators  allows  for  the  replacement  of  the  rigid 
manipulator  arm  design  with  a  flexible  arm  design.  Control  system  design  for 
accurately  and  quickly  positioning  the  flexible  manipulator  provides  another  area 
for  future  research. 

The  minimum  time  to  complete  the  maneuver  was  the  longest  of  the  three 
joint  maneuver  times  and  was  based  on  the  maximum  torque  capabilities  of  the 
actuator  and  the  magnitude  of  the  maneuver.  With  the  near-minimum-time 
reference  tracking  controller,  the  switching  time  for  the  maneuver  was  specified 
to  be  one  half  of  the  maneuver  time.  This  methodology  resulted  in  only  one  of 
the  three  actuators  working  at  maximum  torque.  By  parameterizing  the  near¬ 
minimum-time  equations  in  another  way,  it  may  be  possible  to  optimize  the 
solution  in  terms  of  a  variable  switching  time  and  variable  maneuver  time  for  each 
of  the  actuators.  This  may  allow  optimization  of  the  torques  or  time  required  to 
perform  the  maneuver,  maneuver  time  or  torque  requirements. 
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APPENDIX  A 


TABLE  A.l  Servo  Motor  Characteristics 


Characteristic 

Units 

Manipulator 

Actuator 

Momentum 

Wheel 

Actuator 

Model  Number 

9FGHD 

JR16M4CH-1 

Gear  Reduction  Ratio 

147.51  :  1 

1:1.0 

Rated  Speed 

17 

3000 

Rated  Torque 

80 

31.85 

Rated  Current 

amps 

5  .'6 

7.79 

Rated  Voltage 

volts 

12 

168 

Peak  Torque 

119 

341.43 

Peak  Current 

amps 

62 

79.3 

Peak  Voltage 

volts 

13.2 

7.67 

Outside  Diameter 

inches 

4.75 

7.4 

Height 

inches 

3.1 

4.5 

Weight 

pounds 

3.2 

17.5 

Torque  Constant 

oz-in/amp 

3.23 

69.01 

Back  EMF  Constant 

2.39 

51 

Terminal  Resistance 

ohms 

0.95 

1.4 

Average  Friction  Torque 

oz-in 

2.5 

11 

Viscous  Damping  Constant 

0.3 

7.84 

Moment  Of  Inertia 

oz-in/secA2 

0.0052 

0.084 

APPENDIX  B 


A.  SYSTEM  KINETIC  ENERGY 

1.  Body  1  Kinetic  Energy 

Ti=iiie,2  (B.i) 

2.  Body  2  Kinetic  Energy 

t2  =  l  Icm;  e22  +  i  rn2  l?  e22 

+  m2  Lj  Lc.m.2  0]  02  cos  (02])  (B.2) 

+  i-  m2  Lc.m.2  02 

3.  Body  3  Kinetic  Energy 

t3  =  i  Ic.m.,  e32  +  i  m3  [l?  e,2  +  l2  e22  +  i4.m.,  e32 

+  m3  L]  L2  01  02  cos(02i)+  m3  L3  Lc.m.j  01  03  COS  (03 1)  (B.3) 

+  m3  L2  Lc.m.j  02  03  cos  (032) 

4.  Total  Kinetic  Energy 

T=X T-  (B-4) 

i=l 

B.  EQUATIONS  OF  xMOTION 
1.  Mass  Matrix 

The  mass  matrix  is  a  function  of  the  mass  and  inertia  characteristics  of  the 
system  as  well  as  the  geometry  of  the  system.  The  mass  matrix  is  defined  in 
equation  (B.5)  where  the  elements  of  the  mass  matrix  are  described  as  follows  in 
equations  (B.6)  through  (B.i  1). 
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where 


M(0)  = 


mu  mi2mi3 
11121  m22  m23 
.m^i  11132  11133. 


mn  =  Ii  +  L?  [mi  +  m2  +  m3]  + 1114  L4 


m22  -  Icjn.2  + 


Li 


(iff*”* 


2 

m33  —  Icjn.3  Lcm.3  m3 

mi2  =  m2i  =  Li  L2  cos(02i) 


+  1113 


mi3  =m3i  =  Li  Lcjn.3  cos(03i)m3 
m23  =  m32  =  L2  Lcm3  cos(032)  m3 


2.  Coriolis  And  Centrifugal  Acceleration  Terms: 


G(0,0) 


G(l) 

G(2) 

G(3) 


G(  1 )  =  [  (m2  L 1  Lc  m.2)  +  (m3Li  L2)]  sin  (621)  62  + 
(1TI3L1  Lc.m.3)sin(03i)03 

G(2)  =  -  [  (m2  Li  Lc.m.2)  +  (m3  Li  L2) ]  sin  (02i)  0i  + 
(m3  L2  Lcm.3)  sin  (03i)  ©3 


.  2  . 2 

G(3)  =-  (m3  Li  Lc.m.3)  sin (02i)  0i  -  (m3  L2  Lc  m.3)  sin  (032)  02 


(B.5) 

(B.6) 

(B.7) 

(B.8) 

(B.9) 

(B.10) 

(B.ll) 

(B.12) 

(B.13) 

(B.14) 

(B.15) 
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C  POLYNOMIAL  REFERENCE  TRAJECTORY 

1.  Vector  Polynomial  Describing  Tip  Position: 

r  =  r(to)  + f(t)[r(tf)- r(to)]  (B.16) 

X  =  ^  ,0<t<tf,0<T<l 

tf-  to 

f(x)  =  C  i  +C2l  +  C3T2+  C4T3  +  C5t4  +  C6X5  (B.  17) 

2.  Boundary  Conditions  And  Polynomials 


f(0)  =  0,f(0)  =  0,f(0)=0 
f{l)=  l,f(l)  =  0,f(l)  =  0 

f(x)  =  10x3-  15x4  +  6x5 

(B.18) 

f(x)  =  30  x2  -  60  x3  +  30  x4 

(B.19) 

f(x)  =  60x-  180x2  +  120  x3 

(B.20) 

Vector  Position 

r(x)  =  r  (to)  +  f  (x)  [r  (tf)  -  r  (to)] 

(B.21) 

Vector  Velocity 

'I:*') 

(B.22) 
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(B.23) 


5.  Vector  Acceleration 

1  (tf-to)2 


D.  TWO  LINK  INVERSE  KINEMATICS 

Law  Of  Sines  &  Cosines: 


- 

x2  +  y!  =  +  I2  +  2  ll  I2  COS  (82 

-6,)  (B.24) 

021  =02  -  6] 

(B.25) 

/n  1  x2  +  y2  '  ^  -  12 

COS  1021  /=  of  1 

2  ll  I2 

(B.26) 

sin  (02i  )  =  ±  Vl- cos  (02 iF 

(B.27) 

621  =amn2fsint.e^  H 
\cos(02i  )/ 

(B.28) 

Velocity  Vector. 

+• 

n 

X 

(B.29) 

H  = 

- 12  sin  (02),  - 13  Sin  (©3) 

I2  cos  (62),  I3  cos  (83)  _ 

(B.30) 

Acceleration  Vector: 

r  =  [H]{e)  +  [H](e) 

(B.31) 
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■  [  -  h  cos  (02),  - 13  cos  (63) 

H  = 

-  I2  sin  (02),  - 13  sin  (03) 

E.  NEAR  MINIMUM  TIME  RIGID  BODY  MANEUVER 
1.  Near-Mlnimum-Time  Maneuver 


I  0ref  =  Uref  =  Umax  f(At,tf,t) 

I  -  Moment  Of  Inertia  About  Axis  Of  Rotation 
tf  -  Maneuver  Time 
ti  =ts-At 
t2  =  ts  +  At 
t3  —  tf  -  At 
At  =  a  tf  =  Rise  Time 
ts  =  P  tf  =  Switching  Time,  where  P  =  0.5 
2.  Torque  Shaping  Function 


H2 

3  -  2  f— ) 

for  0  <  t  <  At 

'At' 

\At/J 

=  1  for  At  <  t  <  ti 


[3  -  2  [l  - 1,| 

/  for  t]  <  t  <  t2  ) 

W  2At  1 

'  2At  /J 

'1 

=  -  1  for  t2  <  t  <  t3 


=  -  1  + 


/(Liiif 

[3  -  2  1‘Ail]]) 

W  At  1 

L  \  At  IJI 

0ref(t)  =  0o  +  ^2L 


f(At,tf,x)  dx 


(B.32) 


(B.33) 


(B.34) 


(B.35) 
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-  ['  f 

1  u 


9ref(t)  =  00  +  00  (t-to)  +  I  I  f(At,tf>X2)  dX2  dXj 


(B.36) 


3.  Boundary  Conditions 


*o  =  o;  efo)=e0  e(o)=o 

t  =  tr;  0  (tf)  =  9f  0(tf)  =  0 

Solution  for  the  piece  wise  integration  of  equations  (B.35)  and  (B.36)  for 
the  given  time  interval  provides  us  with  refeence  angular  displacements  and 
angular  velocity. 

4.  Reference  Angular  Displacements  And  Velocities 
0  <  t  <  At 


0ref  =  ^  At2  [l  UX  +  -L  U-f 
I  14  \At)  10  W 


(B.37) 


0ref=^W3-i(f)4 

I  LVAt  ‘  2  \At/ . 


At  <  t  <  t] 


6  ta.  fl  t2  - 1 1  At  +  -2-  (At)5 
I  L2  2  20 1  ; 


eref  =  ymax.(  t-^At 

I  1  2 


(B.38) 


(B.39) 


(B.40) 


I 


tl  <  t  <  t2 


21a2  -  la  + 1 
L20  4  8J 


t?  + 


eref  =  ym^  (2Atf  1  - 1  + 1  . 

I  1  '  1.2  \  2At '  2  \  2At  /  5  '  2At  I  J 

(ltr-|At)(t-ti) 

0rer  =  Y  {..  - 1  At  +  2At  [(^)  -  2  (!^f  +  j^f‘ ]) 


t2  <t<  t3 


Oref  — 


Umax 

I 


Gref  =  Umax 


-t  +  tl  +  t2  -  1  At 
2 


t3  <  t  <  tf 


6 ref  =  ^ 


-1-a2 

20 


1  a  + 1 

2  4. 


tf  +  1  At  (t 
2 


t3)(< 


1 

2 


t-t3\2 , 1  [t-t3\4  1  t-t3 

At  /  4  \  At  /  10  At 


eref  =  Umax  LlAt  +  At 
I  l  2 


(B.41) 


(B.42) 


(B.43) 


(B.44) 


(B.45) 


(B.46) 
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At  time  t  =  tf ; 


eref=tac 


i-J-a  +  J-a2 


14  2 


10 


t2 

tf 


Oref  —  0 


5.  Near-Minimum-Time  Relationships 


tf,  = 


Ii(6fl-QoJ 


“"“'(4-  2  “  +U)“1 


Umax,  — 


ii  (efi  -  6b, ) 


^  f—  -  —  a  +  —  a2) 

2  10  I 


( B.47 ) 


(B.48) 


(B.49) 


(B.50) 
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APPENDIX  C 


A.  LINEAR  FEEDBACK  MATLAB  SIMULATION  PROGRAM 
1.  Main  Program  For  Linear  Feedback  Simulation 

%  Constants 

%  Lenght  of  Manipulators 
L(l)=  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

%  Distance  From  Axis  Of  Rotation  To  Center  Of  Mass 
Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass  Of  Major  Components  (kg) 

m(l)  =  54.69; 

m(2)  =  2.09364; 

m(3)  =  2.466; 

m(4)  =  10.667; 

%  Inertias  Of  Major  Components  (kg-mA2) 

1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 
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%  Integrate  Equations  Of  Motion  Using  Commercial 
%  Matlab  Runge-Kutta  4  Routine 
%  Boundary  Conditions 
tO  =  0; 
tfinal  =  40; 

xO  =  [0.0;  0.0;  0.0;  0*pi/180;  20*pi/180;  40*pi/180]; 
tol  =  le-8; 
trace=l ; 

Call  "lfbrk"  Function  And  Integrate  Equations  Of  Motion 
[t,x,uu]  =  rku2('lfbrk',t0,  tfinal,  xO,tol,trace); 

%  Program  To  Calculate  Motor  Torque,  Current,  and  Voltage 

TF=1.41e-2;  %N-m 

KT=2.28e-2;  %  N-m/amp 

KD=2.00573e-5;  %  N-m/rad/sec 

KE=2.28271e-2;  %  Volt/rad/sec 

RT=0.95;  %  ohms 

j=length(t) 

fori=l:j; 

amp22(i)  =  (uu(i,2)/148.51  +TF  +  KD  *  x(i,2)*148.51 )  /  KT; 
volt22(i)  =  KE  *  x(i,2)  +  RT  *  amp22(i); 
amp33(i)  =  (uu(i,3)/148.51  +  TF  +  KD  “  x(i,3)*148.51  )  /  KT; 
volt33(i)  =  KE  *  x(i,3)  +  RT  *  amp33(i); 
end 
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%  Program  To  Calculate  Motor  Torque,  Current,  and  Voltage 
%  For  Momentum  Wheel 
TF=0.0777;  %N-m/amp 
KD=5.29131e-4;  %  N-m/rad/sec 
KE=0.487 11;  %  Volt/rad/sec 

RT=1.4;  %  ohms 

Iw=0.0912;  %  kg-mA2 

thd0=104.7;  °/o  rad/sec  (=  lOOOrpm) 

fori=l;j; 
tor(i)  =  uu(i,l); 
thddw(i)  =  tor(i)/Iw; 
thdw(i)  =  thddw(i)  *  t(i)  +  thdO; 
amp(i)  =  (tor(i)  +  TF  +  KD  *  thdw(i) )  /  KT; 
volt(i)  =  KE  *  thdw(i)  +  RT  *  amp(i); 
xl(i)=L(l)*cos(x(i,4)); 
yl(i)=L(l  )*sin(x(i,4)); 
x2(i)=xl(i)+L(2)*cos(x(i,5)); 
y2(i)=yl(i)+L(2)*sin(x(i,5)); 
x3(i)=x2(i)+L(3)*cos(x(i,6)); 
y3(i)=y2(i)+L(3)*sin(x(i,6)); 
end 

%  Store  Data  For  Plotting  Later 
datal=[t,x*l  80/pi, uu]; 
data2=[t,xl’,yl,,x2,,y2’,x3',y3']; 
data3=[t,thdw'*30/pi,thddw'*30/pi]; 
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data4=[t,amp'  ,amp22'  ,amp33' ,  volt’  ,volt22',volt33'] ; 

%  Save  Data  In  Text  Format 

save  lfb9.dat  datal  /ascii 

save  lfbl0.dat  data2  /ascii 

save  lfbl  l.dat  data3  /ascii 

save  lfbl2.dat  data4  /ascii 
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2.  Linear  Feedback  Equation  Of  Motion  Function 

%  Function  Containing  Linear  Feedback  Equations  Of  Motion 
function  [xdot.Ul]  =  lfbrk(t,x) 

%  Constants 

%  Lenght  of  Manipulators 
L(l)  =  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

%  Distance  From  Axis  Of  Rotation  To  Center  Of  Mass 
Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass  Of  Major  Components  (kg) 

m(l)  =  54.69; 

m(2)  =  2.09364; 

m(3)  =  2.466; 

m(4)  =  10.667; 

%  Inertias  Of  Major  Components  (kg-mA2) 

1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 
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%  Angles 
thd(l)  =  x(l); 
thd(2)  =  x(2); 
thd(3)  =  x(3); 

%  Convert  Input  Variables  Into  Variable  Used  In  This  Function 
th(l)  =  x(4); 
th(2)  =  x(5); 
th(3)  =  x(6); 

%  Final  Conditions  For  Manipulator  And  Main  Body 
thf  1=0; 

thf2=40*pi/180; 

thf3=60*pi/180; 

thdfl=0; 

thdf2=0; 

thdf3=0; 

%  Linear  Feedback  Control  Law 

glp=.l; 

g2p=i; 

g3p=.l; 

glv=.2; 

g2v=.2; 

g3v=.2; 
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%  Control  Torques 

U(3)=-g3p*(th(3)-thf3)-g3v*(thd(3)-thdf3); 

U(2)=U(3)-g2p*(th(2)-thf2)-g2v*(thd(2)-thdf2); 

U(  1  )=U(2)-g  1  p*(th(  1  )-thf  1  )-g  1  v*(thd(  1  )-thdf  1 ); 

U1=U'; 

%  Calculate  Coeffients  For  The  Equation  Of  Motion  And  Integrate 
[MM,GM]  =  mgm(th,thd); 

B  =  [1,-1,0;0,1,-1;0,0,1]; 
thdd=inv(MM)*(B*U  1  -GM'); 
xdot  =  [thdd;x(l);x(2);x(3)]; 
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B.  POLYNOMIAL  REFERENCE  TRACKING  CONTROLLER 
1.  Polynomial  Reference  Maneuver  Tracker  Main  Program 

%  EOM3.m  Program 
%  Constants 

%  Lenght  of  Manipulators 
L(l)=  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

%  Distance  From  Axis  Of  Rotation  To  Center  Of  Mass 
Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass  Of  Major  Components  (kg) 

mu  J  =  54.69; 

m(2)  =  2.09364; 

m(3)  =  2.466; 

m(4)  =  10.667; 

%  Inertias  Of  Major  Components  (kg-mA2) 

1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  --  5.38398e-2; 

1(4)  =  0.0912; 
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%  Boundary  Conditions  And  Integration  Time 
tO  =  0; 
tfinal  =  10; 

xO  =  [0.0;  0.0;  0.0;  0*pi/180;  20*pi/180;  40*pi/180]; 

tol  =  le-8; 

trace=l; 

%  Integrate  Equations  Of  Motion 

[t,x,uu]  =  rku2(’eom3rk',t0, tfinal, xO.tol, trace); 

j=size(t); 

%  Calculate  Electrical  Power  Requirement  For  Manipulator  Actuators 

%  Motor  Parameters 

TF=1.8e-2  %  N-m 

KT=2.28e-2  %  N-m/amp 

KD=2.00573e-5  %  N-m/rad/sec 

KE=2.28271e-2  %  Volt/rad/sec 

RT=0.95  %  ohms 

j=size(t) 

for  i=l:j; 

amp22(i)  =  (uu(2,i)/148.51  +  TF  +  KD  *  x(i,2) )  /  KT; 
volt22(i)  =  KE  *  x(i,2)  +  RT  *  amp22(i); 
amp33(i)  =  (uu(3,i)/148.51  +  TF  +  KD  *  x(i,3) )  /  KT; 
volt33(i)  =  KE  *  x(i,3)  +  KT  *  amp33(i); 
end 

%  Program  To  Calculate  Motor  Torque,  Current,  and  Voltage 
%  Momentum  Wheel 
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%  Motor  Parameters 


TF=0.0777 

%  N-m 

KT=0.48732 

%  N-m/amp 

KD=5.29131e-4 

%  N-m/rad/sec 

KE=0.48711 

%  Volt/rad/sec 

RT=1.4 

%  ohms 

Iw=0.0912 

%  kg-mA2 

thd0=104.7 

%  rad/sec  (=  lOOOrpm) 

j=size(t) 

for  i=l:j; 

tor(i)  =  uu(l,i); 
thddw(i)  =  tor(i)/Iw; 
thdw(i)  =  tliddw(i)  *  t(i)  +  thdO; 
amp(i)  =  (tor(i)  +  TF  +  KD  *  thdw(i) )  /  KT; 
volt(i)  =  KE  *  thdw(i)  +  RT  *  amp(i); 
end 
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2.  Polynomial  Reference  Tracking  Equations  Of  Motion  Function 

function  [xdot.Ul]  =eom3rk(t,x) 

%  Constants 
L(l)  =  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass 
m(l)  =  54.69; 
m(2)  =  2.09364; 
m(3)  =  2.466; 
m(4)  =  10.667; 

%  Inertia 
1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 

%  Angles 
thd(l)  =  x(l); 
thd(2)  =  x(2); 
thd(3)  =  x(3); 
th(l)  =  x(4); 
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th(2)  =  x(5); 
th(3)  =  x(6); 

[MM,GM]  =  mgm(th,thd); 

%  Gains 

glp=1.0; 
g2p=l  .0; 

g3p=1.0; 

glv=5.0; 

g2v=5.0; 

g3v=5.0; 

[Uref,thref,thdref,thddref]  =  eom3ref(t,L  ,Lcm,m,I); 
dU(3)=-g3p*(th(3)-thref(3))-g3v*(thd(3)-thdref(3)); 
dU(2)=dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); 
dU(  1  )=dU(2)-g  1  p*(th(l)-thref(  1  ))-gl  v*(thd(  1  )-thdref(  1 )); 
Ul=Uref+dU'; 

U=U1'; 

B  =  [1,-1,0;0,1,-1;0,0,1]; 
thdd=inv(MM)*(B*U'-GM'); 
xdot  =  [thdd;x(l);x(2);x(3)3; 


76 


3.  Polynomial  Reference  Trajectory  Function 

function  [Uref,thref,thdref,thddref]  =  eom3ref(t,L,Lcm,mJ); 
thref(l)=0; 

%  Initial  And  Final  Time  For  Maneuver 
tO  =  0; 
tf  =  10; 


%  Initial  And  Final  Vector  Positons 

r3x0  =  L(l)*cos(thref(l))+L(2)*cos(20*pi/180)+L(3)*cos(40*pi/180); 
r3y0  =  L(l)*sin(thref(l))+L(2)*sin(20*pi/180)+L(3)*sin(40*pi/180); 
r3xf  =  L(  1  )*cos(thref(  1  ))+L(2):l£cos(40*pi/l  80)+L(3)*cos(60*pi/1 80); 
r3yf  =  L(l)*sin(thref(l))+L(2)*sin(40*pi/180)+L(3)*sin(60*pi/180); 


%  Calculate  Reference  Maneuver 
tau  =  ( t  -  tO)  /  ( tf  -  tO ); 

r3x  =  r3x0  +  (  rixf  -  r3x0  )  *  (  10  *  tauA3  -  15  *  tauA4  +  6  *  tauA5  ); 
r3y  =  r3y0  +  (  r3yf  -  r3y0  )  *  (  10  *  tauA3  -  15  *  tauA4  +  6  *  tauA5  ); 
r3xd  =  (  r3xf  -  r3x0  )  /  ( tf  -  tO  )*  (  30  *  tauA2  -  60  *  tauA3  +  30  *  tauA4); 
r3yd  =  (  r3yf  -  r3y0  )  /  ( tf  -  tO  )*(  30  *  tauA2  -  60  *  tauA3  +  30  *  tauA4); 
r3xdd  =  (r3xf-r3x0)/((tf-t0)A2)*(60*tau-180*tauA2+120*tauA3); 
r3ydd  =  (r3yf-r3y0)/((tf-t0)A2)*(60*tau-180*tauA2+120*tauA3); 
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if  (t>tf); 
r3x=0; 
r3y=0; 
r3xd=0; 
r3yd=0; 
r3xdd=0; 
r3ydd=0; 
end 

%  Determine  Inverse  Kinematics 

th23  =  kink(r3x-L(l)*cos(thref(l)),r3y-L(l)*sin(thref(l)),L(2),L(3)); 
thref(2)  =  th23(l); 
thref(3)  =  th23(2); 

%  Calculate  Joint  Velocites  Using  Jacobean 
H  =  [-L(2)*  sin(thref(2)),-L(3)*  sin(thref(3 )); .. . . 

L(2)*cos(thref(2)),  L(3)*cos(thref(3))]; 
thd23  =  inv(H)  *  f  r3xd;  r3yd  ]; 
thdref(2)  =  thd23(l); 
thdref(3)  =  thd23(2); 

%  Calculate  Joint  Acceleration  Using  Jacobean 
Hdot=[-L(2)*thdref(2)*cos(thref(2)),-L(3)*thdref(3)*cos(thref(3));.... 
-L(2)*thdref(2)*sin(thref(2)),-L(3)*thdref(3)*sin(thref(3))]; 
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thdd23=inv(H)*([r3xdd;r3ydd]-Hdot*[thdref(2);thdref(3)]); 
thddref(2)  =  thdd23(l); 
thddref(3)  =  thdd23(2); 

%  Calculate  Reference  Control  Torques 
[MM,GM]  =  mgm(thref,thdref); 

B  =  [1,-1,0;0,1,-1;0,0,1]; 

Uref  =  inv(B)  *  ( MM  *  thddref  +  GM’ ); 
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C  NEAR-MINIMUM-TIME  RERENCE  TRACKING  CONTROLLER 
1.  Main  Program 

%  nmtl  program 
%  Constants 
L(l)=  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass 
m(l)  =  54.69; 
m(2)  =  2.09364; 
m(3)  =  2.466; 
m(4)  =  10.667; 

%  Inertia 
1(1)  =4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 

%  Maximum  Torque 
umax=0.300; 

%  Shaping  Function  Coefficient 
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alfa=0. 1 ; 
beta=0.5; 

t0=0; 

MM(1,1)  =  I(l)+L(l)A2*(m(2)+m(3))+L(4)A2*m(4); 
MM(2,2)  =  1(2)  +  m(2)  *  Lcm(2)A2  +  m(3)  *  L(2)A2; 
MM(3,3)  =  1(3)  +  m(3)  *  Lcm(3)A2; 


%  Initial  And  Final  Manipulator  Position 
th0=[0*pi/180;20*pi/180;40*pi/180]; 
thf=[0.01*pi/l  80;40*pi/l  80;60*pi/l 80] ; 


%  Determine  Minimum  Time  To  Perform  Maneuver 
T(  1  )=sqrt(MM(  1 , 1  )*(thf(l)-th0(  1 ))/... 

(umax(l)*(l/4-l/2*aIfa+l/10*aIfaA2))); 

T(2)=sqrt(MM(2,2)*(thf(2)-thO(2))/... 

(umax(  1  )*(  1  /4- 1  /2*alfa+l  /1 0*alfaA2))); 
T(3)=sqrt(MM(3,3)*(thf(3)-th0(3))/... 

(umax(l)*(l/4-l/2*alfa+l/10*alfaA2))) 

T_max=max(T) 

tf=T_max 

delt=alfa*tf 

tf=T_max 

dt=alfa*tf 

ts=beta*tf 

tl=ts-dt 
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t2=ts+dt 

t3=tf-dt 

echo  off; 

t0=0; 

trace=l; 

tol=le-8; 

x0=[0;0;0;0*pi/l  80;20*pi/l  80;40*pi/l  80]; 

%  Integrate  Equations  Of  Motion 
[t,x,uu]  =  rku2('nmtrk',t0,tf)x0,tol, trace); 
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2.  Near-Minimum-Time  Equations  Function 
function  [xdot,U]  =  nmtrk(t,x); 

umax(l)=0.300; 

%  Constants 

L(l)  =  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass 

m(l)  =  54.69; 
m(2)  =  2.09364; 
m(3)  =  2.466; 
m(4)  =  10.667; 

%  Inertia 
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1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 

%  Angles 

thd(l)  =  x(l); 
thd(2)  =  x(2); 
thd(3)  =  x(3); 

th(l)  =  x(4); 
th(2)  =  x(5); 
th(3)  =  x(6); 

[MM.GM]  =  mgm(th,thd); 

%  Gains 
glp=.10; 
g2p=.10; 

g3p=.10; 

glv=.50; 

g2v=.50; 

g3v=.50; 
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[MM,GM]  =  mgm(th,thd); 


[Uref,thref,thdref,thddref]  =  nmt_ref(t); 

dU(3)=-g3p*(th(3)-thref(3))-g3v*(thd(3)-thdref(3)); 

dU(2)=dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); 

dU(  1  )=dU(2)-g  1  p*(th(  1  )-thref(  1  ))-g  1  v  *  (thd(  1  )-thdref(  1 )); 

Ul=Uref+dU’; 

U=U1'; 

B  =  [i,-i,0;0,i,-i;0,0,i]; 

xdot  =  [  inv(MM)  *  ( B  *  U'  -  GM' );  x(l);  x(2);  x(3)  ]; 
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3.  Near-Minimum-Time  Reference  Function 


%  Reference  Generator  Function 

function  [Uref,thref,thdref,thddref]  =  nmt_ref(t); 

umax(l)=0.300; 

%  Constants 
L(l)=  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 


%  Mass 
m(l)  =  54.69; 
m(2)  =  2.09364; 
m(3)  =  2.466; 
m(4)  =  10.667; 

%  Inertia 
1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.0912; 

MM(l.l)  =  I(  1  )+L(  1  )A2*(m(2)+m(3))+L(4)A2*m(4); 
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MM(2,2)  =  1(2)  +  m(2)  *  Lcm(2)A2  +  m(3)  *  L(2)A2; 

MM(3,3)  =  1(3)  +  m(3)  *  LcmO^; 
th0=[0*pi/l  80;20*pi/l  80;40*pi/l  80] ; 
thf=[0.01*pi/180;40*pi/180;60*pi/180]; 
alfa=0.1; 

T(  1  )=sqrt(MM(  1 , 1  )*(thf(  1  )-th0(  1  ))/(umax(  1  )* ... 

(l/4-l/2*alfa+l/10*alfaA2))); 

T(2)=sqrt(MM(2,2)*(thf(2)-thO(2))/(umax(l)*... 

(l/4-l/2*alfa+l/10*alfaA2))); 

T(3)=sqrt(MM(3,3)*(thf(3)-thO(3))/(umax(l)*... 

(l/4-l/2*alfa+l/10*alfaA2))); 

T_max=max(T); 

tf=T_max; 

dt=alfa*T_max; 

tl=tf/2-dt; 

t2=tf/2+dt; 

t3=tf-dt; 

umax(3)=MM(3,3)*(thf(3)-th0(3))/... 

(tf A2*(  1 /4- 1 /2*alfa+l /1 0*  alfaA2)); 
umax(2)=MM(2,2)*(thf(2)-thO(2))/... 
(tfA2*(l/4-l/2*alfa+l/10*alfaA2)); 

umax(  1  )=MM(  1 , 1  )*(thf(  1  )-thO(  1  ))/(tfA2*(  1 /A- 1  /2*alfa+ 1  /1 0*alfaA2)); 

%  Near  Minimum  Time  Reference  Maneuver 
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if  (t>=0  &  t<=dt); 
f=(t/dt)A2*(3-2*(t/dt)); 
ff=dt*((t/dt)A3-(l/2)*(t/dt)A4); 
fff=(dtA2)*((l/4)*(t/dt)A4-(l/10)*(t/dt)A5); 


elseif  (t>dt  &  t<=tl ); 
f=  l; 

ff=(t-(l/2)*dt); 

fff=((  l/2)*tA2-(  1  /2)*t*dt+(3/20)*dtA2); 


elseif  (t>tl  &  t<=t2); 

f  =  1  -2*(((t-tl)/(2*dt))A2*(3-2*((t-tl)/(2*dt))»; 
ff  =  -(l/2)*dt+tl+2*dt*(((t-tl)/(2*dt))-2*((t-tl)/(2*dt))A3+... 
((t-tl  )/(2*dt))A4); 

fff  =  ((23/20)*alfaA2-(3/4)*alfa+(l/8))*tfA2+(2*dt)A2*... 
((l/2)*((t-tl)/(2*dt))A2-(l/2)*((t-tl)/(2*dt))A4... 
+(l/5)*((t-tl)/(2*dt))A5)+((l/2)*tf-(3/2)*dt)*(t-tl); 


elseif  (t>t2  &  t<=t3); 
f=-i; 

ff=-t+tl+t2-(l/2)*dt+2*dt*(((t2-tl)/(2*dt))-... 

2*((t2-tl)/(2*dt))A3+((t2-tl)/(2*dt))A4); 
fff=(-(21/20)*alfaA2+(l/4)*alfa+l/8)*tfA2+... 
(l/2)*(tf-3*dt)*(t-t2)-(l/2)*(t-(  1  /2)*tf-dt)A2; 
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elseif  (t>t3  &  t<=tf); 
f  =  -l+(((t-t3)/(dt))A2*(3-2*((t-t3)/dt))); 
ff=((l/2)*dt+dt*(-(t-t3)/dt+((t-t3)/dt)A3-(l/2)*((t-t3)/dt)A4)); 
fff=(-(l/20)*alfaA2-(l/2)*alfa+l/4)*tfA2+(l/2)*dt*(t-t3)+... 
dtA2*(-(l/2)*((t-6)/dt)A2+(l/4)*((t-t3)/dt)A4-(l/10)*((t-t3)/dt)A5); 


elseif  (t>tf); 

f=0; 

ff=0; 

fff=0; 


end 


thref(  1  )=(umax(  1  )/MM(  1  ,l))*fff+thO(l ); 

thref(2)=(umax(2)/MM(2,2))*fff+thO(2); 

thref(3)=(umax(3)/MM(3,3))*fff+thO(3); 

thdref(  1  )=(umax(  1  )/MM(  1 , 1 
thdref(2)=(umax(2)/MM(2,2))*ff; 
thdref(3)=(umax(3)/MM(3 ,3))*ff ; 


thddref(  1  )=umax(  1  )*f/MM(  1 , 1 ); 
thddref(2)=umax(2)*f/MM(2,2); 
thddref(3)=umax(3)*f/MM(3,3); 
[MM,GM]  =  mgm(thdref,thref); 
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B  =  [1,-1,0;0,1,-1;0,0,1]; 

Uref  =  inv(B)  *  (  MM  *  thddref  +  GM’ ); 
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D.  MISCELLANEOUS  FUNCITONS 

1.  Equation  Of  Motion  Coefficient  Funciton 
function  [MM,GM]  =  mgm(th,thd); 

%  Constants 
L(l)=  15*2.54/100; 

L(2)  =  17*2.54/100; 

L(3)  =  17*2.54/100; 

L(4)  =  20/100; 

Lcm(l)  =  0/100; 

Lcm(2)  =  36.45/100; 

Lcm(3)  =  34.9/100; 

%  Mass 
m(l)  =  54.69; 
m(2)  =  2.09364; 
m(3)  =  2.466; 
m(4)  =  10.667; 

%  Inertia 
1(1)  =  4.32132; 

1(2)  =  3.20338e-2; 

1(3)  =  5.38398e-2; 

1(4)  =  0.00912; 
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%  Subroutine  To  Calculate  Mass  and  "G”  Matrix 
th21  =  th(2)  -  th(l); 
th31  =  th(3)  -  th(l); 
th32  =  th(3)  -  th(2); 


%  'Mass’  Matrix 

MM(1,1)  =  1(1  )+L(l  )A2*(m(2)+m(3))+L(4)A2*m(4); 

MM(1,2)  =  ((m(2)*L(l)*Lcm(2))+(m(3)*L(l)*L(2)))*cos(th21); 
MM(1,3)  =  m(3)*L(l)*Lcm(3)*cos(th31); 


MM(2,1)  =  MM(1,2); 

MM(2,2)  =  1(2)  +  m(2)  *  Lcm(2)A2  +  m(3)  *  L(2)A2; 
MM(2,3)  =  m(3)  *  L(2)  *  Lcm(3)  *  cos(th32); 


MM(3,1)  =  MM(1,3); 

MM(3,2)  =  MM(2,3); 

MM(3,3)  =  K3)  +  m(3)  *  1x01(3^2; 

%  'G' Matrix 

cl  22  =  ((m(2)*L(l)*Lcm(2))+(m(3)*L(l)*L(2)))*sin(th21); 
cl33  =  m(3)*L(  1  )*Lcm(3)*sin(th3 1 ); 

c211  =-  ((m(2)*L(l),,<Lcm(2))+(m(3)*L(l)*L(2)))*sin(th21); 
c233  =  m(3)*L(2)*Lcm(3)*sin(th32); 


92 


c311  =  -  m(3)*L(  1  )*Lcm(3)*sin(th3 1 ); 
c322  =  -  m(3)*L(2)*Lcm(3)*sin(th32); 


cl=[0,0,0;... 

0,cl22,0;... 

0,0,cl33]; 


c2=[c21 1,0,0;... 
0,0,0;... 
0,0,c233]; 


c3=[c31 1,0,0;... 
0,c322,0;... 

0,0,0]; 

GM(1)  =  thd  *  cl  *  thd'; 
GM(2)  =  thd  *  c2  *  thd'; 
GM(3)  =  thd  *  c3  *  thd’; 


93 


2.  Inverse  Kinematics  Function 

function  theta =kink(x,y,Ll  ,L2) 

%  Subroutine  To  Determine  Inverse  Kinematic  Solution 

cl2=(xA2+yA2-LlA2-L2A2)/(2*Ll*L2); 

sl2=sqrt(l-cl2A2); 

theta  1 2=atan2(s  1 2,c  1 2) ; 

minv=inv([Ll+L2*cl2,-L2*sl2;L2*sl2,Ll+L2*cl2n; 

c=minv*[x;y]; 

theta(  1  )=atan2(c(2),c(  1 )) ; 

theta(2)=theta(  1  )+theta  1 2; 
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Angular  Displacement  (Deg) 


APPENDIX  D 


A.  LINEAR  CONSTANT  GAIN  FEEDBACK  CONTROLLER 


Figure  D.l  Linear  Constant  Gain  Feedback  Joint  Position  Time  History  With 

Gp=0.1  And  Gv=0.2 


Angular  Velocity  (Deg/Sec 


Figure  D.2  Linear  Constant  Gain  Feedback  Joint  Velocity  Time  History 

With  Gp=0.1  And  Gv=0.2 
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Torque  (N-m) 


Figure  D.3  Linear  Constant  Gain  Feedback  Torque  Time  History  With 

Gp=0.1  And  Gy=0.2 
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Wheel  Speed  (N-m) 


Figure  D.4  Linear  Constant  Gain  Feedback  Momentum  Wheel  Speed  Time 

History  With  Gp=0.1  And  Gy=0.2 
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Angular  Displacement  (Dec) 


B.  POLYNOMIAL  REFERENCE  TRACKING  CONTROLLER 
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Figure  D.5  Polynomial  Reference  Tracking  Controller  Joint  Position  Time 
History  With  Gp=I  and  Gv=5  And  Maneuver  Time  Of  5  Seconds 
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Angular  Velocity  (Deg/Sec) 


Figure  D.6  Polynomial  Reference  Tracking  Controller  Joint  Velociy  Time 
History  With  Gp=l  and  Gv=5  And  Maneuver  Time  Of  5  Seconds 
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Torque 


Figure  D.7  Polynomial  Reference  Tracking  Controller  Torque  Time  History 
With  Gp=l  and  Gy=5  And  Maneuver  Time  Of  5  Seconds 


101 


Torque  (N-tn) 


Figure  D.8  Polynomial  Reference  Tracking  Controller  Momentum  Wheel 
Speed  Time  History  With  Gp=l  and  Gv^S  And  Maneuver  Time  Of  5  Seconds 
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Angular  Displacement  (Deg) 


C.  NEAR-MINIMUM-TIME  REFERENCE  TRACKING  CONTROLLER 


Figure  D.9  Near-Minimum  Time  Tracking  Controller  Joint  Position  Time 
History  With  Gp=l ,  Gy=5,  Maneuver  Time  Of  5  Seconds  ,  And  a  =  0.25 


Angular  Velocity  (Deg/Sec) 


Figure  D.10  Near-Minimum  Time  Tracking  Controller  Joint  Velocity  Time 
History  With  Gp=l ,  Gv=5,  Maneuver  Time  Of  5  Seconds ,  And  a  =  0.25 
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Figure  D.U  Near-Minimum  Time  Tracking  Controller  Torque  Time  History 
With  Gp=l ,  Gv=5,  Maneuver  Time  Of  5  Seconds ,  And  a  =  0.25 
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Figure  D.12  Near-Minimum  Time  Tracking  Controller  Wheel  Speed  Time 
History  With  Gp=l ,  Gy=5,  Maneuver  Time  Of  5  Seconds  ,  And  a  =  0.25 


106 


Angular  Displacement  (Deg) 


Figure  D.13  Near-Minimum  Time  Tracking  Controller  Joint  Position  Time 
History  With  Gp=l ,  Gy=5,  Maneuver  Time  Of  5  Seconds  ,  And  a  =  0.1 
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Angular  Velocity  (Deg/Sec) 


Figure  D.14  Near-Minimum  Time  Tracking  Controller  Joint  Velocity  Time 
History  With  Gp=l ,  Gy=5,  Maneuver  Time  Of  5  Seconds ,  And  a  =  0.1 
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Torque  (N-m) 


Figure  D.15  Near-Minimum  Time  Tracking  Controller  Torque  Time  History 
With  Gp=l ,  Gy-5,  Maneuver  Time  Of  5  Seconds ,  And  a  =  0.1 
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Figure  D.16  Near-Minimum  Time  Tracking  Controller  Wheel  Speed  Time 
History  With  Gp=l ,  Gy=5,  Maneuver  Time  Of  5  Seconds  ,  And  a  =  0.1 
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